dev

.gitignore

+/build/
\ No newline at end of file

package.xml

+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>rq_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="simon.wittl@th-deg.de">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

rq_controller/common/__init__.py

+from .projection import PyProjectionGeometry, PyProjection
+from .region_of_intrest import PyRegionOfIntrest
\ No newline at end of file

rq_controller/common/io/loader.py

+import numpy as np
+import json
+from pathlib import Path
+from ...common import PyProjectionGeometry, PyProjection, PyRegionOfIntrest
+
+
+class BaseDataLoader():
+    def __init__(self, 
+                 porjection_geometry_suffix: str, 
+                 projection_suffix: str,
+                 region_of_intrest_suffix: str):
+        
+        self.porjection_geometry_suffix = porjection_geometry_suffix
+        self.projection_suffix = projection_suffix
+        self.region_of_intrest_suffix = region_of_intrest_suffix
+
+    def load_projection_geometry(self, load_path: Path) -> PyProjectionGeometry:
+        raise NotImplementedError
+    
+    def load_projection(self, load_path: Path) -> PyProjection:
+        raise NotImplementedError
+    
+    def load_region_of_intrest(self, load_path: Path) -> PyRegionOfIntrest:
+        raise NotImplementedError
+    
\ No newline at end of file

rq_controller/common/io/rq_json/load.py

+import numpy as np
+import json 
+from pathlib import Path
+
+from ..loader import BaseDataLoader, PyProjection, PyProjectionGeometry, PyRegionOfIntrest
+from PIL import Image
+
+
+class RqJsonLoader(BaseDataLoader):
+    def __init__(self):
+        super().__init__('.geom-json', '.tif', '.roi-json')
+
+    def load_json(self, load_path: Path) -> dict:
+        with open(str(load_path), 'r') as f:
+            data_dict = json.load(f)
+        return data_dict
+
+    def load_projection_geometry(self, load_path: Path) -> PyProjectionGeometry:
+        data_dict = self.load_json(load_path)
+
+        projection_geometry = PyProjectionGeometry(
+            focal_spot_mm=np.array([data_dict['focal_spot_mm']]),
+            detector_postion_mm=np.array([data_dict['detector_postion_mm']]),
+            detector_orientation_quad=np.array([data_dict['detector_orientation_quad']]),
+            frame_id=data_dict['frame_id'])
+        
+        return projection_geometry
+    
+    def load_projection(self, load_path: Path) -> PyProjection:
+        load_path_projection_geometry = load_path.parent / f'{load_path.stem}.{self.porjection_geometry_suffix}'
+        data_dict = self.load_json(load_path_projection_geometry)
+        image = np.asarray(Image.open(load_path))
+
+        projection = PyProjection(
+            focal_spot_mm=np.array([data_dict['focal_spot_mm']]),
+            detector_postion_mm=np.array([data_dict['detector_postion_mm']]),
+            detector_orientation_quad=np.array([data_dict['detector_orientation_quad']]),
+            image=image,
+            detector_heigth_mm=data_dict['detector_heigth_mm'],
+            detector_width_mm=data_dict['detector_width_mm'],
+            frame_id=data_dict['frame_id'])
+        
+        return projection
+    
+    def load_region_of_intrest(self, load_path: Path) -> PyRegionOfIntrest:
+        data_dict = self.load_json(load_path)
+
+        region_of_intrest = PyRegionOfIntrest(
+            start_point_mm=np.array(data_dict['start_point_mm']),
+            end_point_mm=np.array(data_dict['end_point_mm']))
+        
+        return region_of_intrest

rq_controller/common/io/rq_json/write.py

+import numpy as np
+import json 
+from pathlib import Path
+
+from ..writer import BaseDataWriter, PyProjection, PyProjectionGeometry, PyRegionOfIntrest
+from PIL import Image
+
+
+class RqJsonWriter(BaseDataWriter):
+    def __init__(self):
+        super().__init__('.geom-json', '.tif', '.roi-json')
+
+    def write_json(self, save_path: Path, data_dict: dict):
+        with open(str(save_path), 'w') as f:
+            json.dump(data_dict, f, indent=2)
+
+    def write_projection_geometry(self, save_path: Path, projection_geometry: PyProjectionGeometry):
+        data_dict = dict()
+        data_dict['focal_spot_mm'] = projection_geometry.focal_spot_mm.tolist()
+        data_dict['detector_postion_mm'] = projection_geometry.detector_postion_mm.tolist()
+        data_dict['detector_orientation_quad'] = projection_geometry.detector_orientation_quad.tolist()
+        data_dict['frame_id'] = projection_geometry.frame_id
+
+        self.write_json(save_path, data_dict)
+
+    def write_projection(self, save_path: Path, projection: PyProjection):
+        save_path_projection_geometry = save_path.parent / f'{save_path.stem}.{self.porjection_geometry_suffix}'
+        data_dict = dict()
+        data_dict['focal_spot_mm'] = projection.focal_spot_mm.tolist()
+        data_dict['detector_postion_mm'] = projection.detector_postion_mm.tolist()
+        data_dict['detector_orientation_quad'] = projection.detector_orientation_quad.tolist()
+        data_dict['pixel_pitch_x_mm'] = projection.pixel_pitch_x_mm
+        data_dict['pixel_pitch_y_mm'] = projection.pixel_pitch_y_mm
+        data_dict['detector_heigth_mm'] = projection.detector_heigth_mm
+        data_dict['detector_width_mm'] = projection.detector_width_mm
+        data_dict['frame_id'] = projection.frame_id
+
+        self.write_json(save_path_projection_geometry, data_dict)
+
+        image = Image.fromarray(projection.image)
+        image.save(save_path)
+
+    def write_region_of_intrest(self, save_path: Path, region_of_intrest: PyRegionOfIntrest):
+        data_dict = dict()
+        data_dict['focal_spot_mm'] = region_of_intrest.start_point_mm.tolist()
+        data_dict['detector_postion_mm'] = region_of_intrest.end_point_mm.tolist()
+        data_dict['frame_id'] = region_of_intrest.frame_id
+
+        self.write_json(save_path, data_dict)

rq_controller/common/io/writer.py

+import numpy as np
+import json
+from pathlib import Path
+from ...common import PyProjectionGeometry, PyProjection, PyRegionOfIntrest
+
+
+class BaseDataWriter():
+    def __init__(self, 
+                 porjection_geometry_suffix: str, 
+                 projection_suffix: str,
+                 region_of_intrest_suffix: str):
+        
+        self.porjection_geometry_suffix = porjection_geometry_suffix
+        self.projection_suffix = projection_suffix
+        self.region_of_intrest_suffix = region_of_intrest_suffix
+
+    def write_projection_geometry(self, save_path: Path, projection_geometry: PyProjectionGeometry):
+        raise NotImplementedError
+    
+    def write_projection(self, save_path: Path, projection: PyProjection):
+        raise NotImplementedError
+    
+    def write_region_of_intrest(self, save_path: Path, region_of_intrest: PyRegionOfIntrest):
+        raise NotImplementedError
+    
+    def get_projection_geometry_geometry_save_path(self, save_folder: Path, save_name: str) -> Path:
+        raise NotImplementedError
+    
+    def get_projection_geometry_save_path(self, save_folder: Path, save_name: str) -> Path:
+        raise NotImplementedError
+    
+    def get_region_of_intrest_save_path(self, save_folder: Path, save_name: str) -> Path:
+        raise NotImplementedError
+    
+    def number_of_projection_geometries(self, folder: Path) -> int:
+        return len(list(folder.glob(f'*{self.porjection_geometry_suffix}')))
+    
+    def number_of_projections(self, folder: Path) -> int:
+        return self.number_of_projection_geometries(folder)
+

rq_controller/common/projection.py

+import numpy as np
+
+from numpy import ndarray
+from .projection_geometry import PyProjectionGeometry
+
+from rq_interfaces.msg import Projection, ProjectionGeometry
+
+class PyProjection(PyProjectionGeometry):
+    def __init__(self, focal_spot_mm: ndarray, detector_postion_mm: ndarray, detector_orientation_quad: ndarray, image: np.ndarray,
+                 detector_heigth_mm: float, detector_width_mm: float, frame_id: str = 'object') -> None:
+        super().__init__(focal_spot_mm, detector_postion_mm, detector_orientation_quad, frame_id)
+        self.image = image
+        self.detector_heigth_mm = detector_heigth_mm
+        self.detector_width_mm = detector_width_mm
+
+    @classmethod
+    def dummy(cls):
+        return cls(np.array([1., 0., 0]), 
+                   np.array([-1., 0., 0]),
+                   np.array([1., 0., 0, 1.]),
+                   np.zeros((10, 10)),
+                   10., 10.)
+
+    @classmethod
+    def from_message(cls, msg: Projection):
+        focal_spot_mm = np.array([msg.projection_geometry.focal_spot_postion_mm.x,
+                                  msg.projection_geometry.focal_spot_postion_mm.y,
+                                  msg.projection_geometry.focal_spot_postion_mm.z,])
+
+        detector_center_mm = np.array([msg.projection_geometry.detector_postion_mm.x,
+                                       msg.projection_geometry.detector_postion_mm.y,
+                                       msg.projection_geometry.detector_postion_mm.z,])
+        
+        detector_orientation_quad = np.array([msg.projection_geometry.detector_orientation_quad.x,
+                                              msg.projection_geometry.detector_orientation_quad.y,
+                                              msg.projection_geometry.detector_orientation_quad.z,
+                                              msg.projection_geometry.detector_orientation_quad.w])
+        
+        image = msg.image
+        detector_heigth_mm = msg.detector_heigth_mm
+        detector_width_mm = msg.detector_width_mm
+        frame_id = msg.projection_geometry.header.frame_id
+
+        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, image, detector_heigth_mm, detector_width_mm, frame_id)
+    
+    def as_message(self) -> ProjectionGeometry:
+        message = Projection()
+        projection_geometry = ProjectionGeometry()
+
+        projection_geometry.focal_spot_postion_mm.x = self.focal_spot_mm[0]
+        projection_geometry.focal_spot_postion_mm.y = self.focal_spot_mm[1]
+        projection_geometry.focal_spot_postion_mm.z = self.focal_spot_mm[2]
+
+        projection_geometry.detector_postion_mm.x = self.detector_postion_mm[0]
+        projection_geometry.detector_postion_mm.y = self.detector_postion_mm[1]
+        projection_geometry.detector_postion_mm.z = self.detector_postion_mm[2]
+
+        projection_geometry.detector_orientation_quad.x = self.detector_orientation_quad[0]
+        projection_geometry.detector_orientation_quad.y = self.detector_orientation_quad[1]
+        projection_geometry.detector_orientation_quad.z = self.detector_orientation_quad[2]
+        projection_geometry.detector_orientation_quad.w = self.detector_orientation_quad[3]
+
+        message.projection_geometry = projection_geometry
+        message.image = self.image
+        message.detector_heigth_mm = self.detector_heigth_mm
+        message.detector_width_mm = self.detector_width_mm
+
+        message.projection_geometry.header.frame_id = self.frame_id
+
+        return message
+    
+    @property
+    def detector_heigth_px(self) -> int:
+        return self.image.shape[0]
+    
+    @property
+    def detector_width_px(self) -> int:
+        return self.image.shape[1]
+    
+    @property
+    def pixel_pitch_x_mm(self) -> float:
+        return self.detector_width_mm / self.detector_width_px
+    
+    @property
+    def pixel_pitch_y_mm(self) -> float:
+        return self.detector_heigth_mm / self.detector_heigth_px
\ No newline at end of file

rq_controller/common/projection_geometry.py

+import numpy as np
+
+from rq_interfaces.msg import ProjectionGeometry
+
+class PyProjectionGeometry():
+    def __init__(self, focal_spot_mm: np.ndarray, detector_postion_mm: np.ndarray, detector_orientation_quad: np.ndarray,
+                 frame_id: str = 'object') -> None:
+        self.focal_spot_mm = focal_spot_mm
+        self.detector_postion_mm = detector_postion_mm
+        self.detector_orientation_quad = detector_orientation_quad
+        self.frame_id = frame_id
+
+    @classmethod
+    def dummy(cls):
+        return cls(np.array([1., 0., 0]), 
+                   np.array([-1., 0., 0]),
+                   np.array([1., 0., 0, 1.]))
+
+    @classmethod
+    def from_message(cls, msg: ProjectionGeometry):
+        focal_spot_mm = np.array([msg.focal_spot_postion_mm.x,
+                                  msg.focal_spot_postion_mm.y,
+                                  msg.focal_spot_postion_mm.z,])
+
+        detector_center_mm = np.array([msg.detector_postion_mm.x,
+                                       msg.detector_postion_mm.y,
+                                       msg.detector_postion_mm.z,])
+        
+        detector_orientation_quad = np.array([msg.detector_orientation_quad.x,
+                                              msg.detector_orientation_quad.y,
+                                              msg.detector_orientation_quad.z,
+                                              msg.detector_orientation_quad.w])
+        frame_id = msg.header.frame_id
+
+        return cls(focal_spot_mm, detector_center_mm, detector_orientation_quad, frame_id)
+    
+    def as_message(self) -> ProjectionGeometry:
+        message = ProjectionGeometry()
+
+        message.focal_spot_postion_mm.x = self.focal_spot_mm[0]
+        message.focal_spot_postion_mm.y = self.focal_spot_mm[1]
+        message.focal_spot_postion_mm.z = self.focal_spot_mm[2]
+
+        message.detector_postion_mm.x = self.detector_postion_mm[0]
+        message.detector_postion_mm.y = self.detector_postion_mm[1]
+        message.detector_postion_mm.z = self.detector_postion_mm[2]
+
+        message.detector_orientation_quad.x = self.detector_orientation_quad[0]
+        message.detector_orientation_quad.y = self.detector_orientation_quad[1]
+        message.detector_orientation_quad.z = self.detector_orientation_quad[2]
+        message.detector_orientation_quad.w = self.detector_orientation_quad[3]
+
+        message.header.frame_id = self.frame_id
+
+        return message
\ No newline at end of file

rq_controller/common/region_of_intrest.py

+import numpy as np
+
+from rq_interfaces.msg import RegionOfIntrest
+
+
+class PyRegionOfIntrest():
+    def __init__(self, start_point_mm: np.ndarray, end_point_mm: np.ndarray, frame_id: str = 'object'):
+        self.start_point_mm = start_point_mm
+        self.end_point_mm = end_point_mm
+        self.frame_id = frame_id
+
+    @classmethod
+    def dummy(cls):
+        return cls(np.ones(3,) * -1, np.ones(3,))
+
+    @classmethod
+    def from_message(cls, msg: RegionOfIntrest):
+        start_point = np.array([msg.start_point_mm.x,
+                                msg.start_point_mm.y,
+                                msg.start_point_mm.z])
+        
+        end_point = np.array([msg.end_point_mm.x,
+                              msg.end_point_mm.y,
+                              msg.end_point_mm.z])
+        frame = msg.header.frame_id
+        
+        return cls(start_point, end_point, frame)
+    
+    def as_message(self) -> RegionOfIntrest:
+        message = RegionOfIntrest()
+
+        message.start_point_mm.x = self.start_point_mm[0]
+        message.start_point_mm.y = self.start_point_mm[1]
+        message.start_point_mm.z = self.start_point_mm[2]
+
+        message.start_point_mm.x = self.start_point_mm[0]
+        message.start_point_mm.y = self.start_point_mm[1]
+        message.start_point_mm.z = self.start_point_mm[2]
+
+        message.header.frame_id = self.frame_id
+
+        return message

setup.cfg

+[develop]
+script_dir=$base/lib/rq_controller
+[install]
+install_scripts=$base/lib/rq_controller

setup.py

+from setuptools import find_packages, setup
+
+package_name = 'rq_controller'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='root',
+    maintainer_email='simon.wittl@th-deg.de',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+        ],
+    },
+)

test/test_copyright.py

+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

test/test_flake8.py

+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

test/test_pep257.py

+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'