diff --git a/.github/dependabot.yml b/.github/dependabot.yml new file mode 100644 index 0000000..05a48fc --- /dev/null +++ b/.github/dependabot.yml @@ -0,0 +1,13 @@ +# To get started with Dependabot version updates, you'll need to specify which +# package ecosystems to update and where the package manifests are located. +# Please see the documentation for all configuration options: +# https://docs.github.com/github/administering-a-repository/configuration-options-for-dependency-updates + +version: 2 +updates: + - package-ecosystem: "github-actions" + # Workflow files stored in the + # default location of `.github/workflows` + directory: "/" + schedule: + interval: "weekly" diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml new file mode 100644 index 0000000..2c0c0de --- /dev/null +++ b/.github/workflows/ci-format.yml @@ -0,0 +1,23 @@ +# This is a format job. Pre-commit has a first-party GitHub action, so we use +# that: https://github.com/pre-commit/action + +name: Format + +on: + workflow_dispatch: + pull_request: + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5.1.0 + with: + python-version: '3.10' + - name: Install system hooks + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.1 + with: + extra_args: --all-files --hook-stage manual diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b4cff9d --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +*.pyc +*.__pycache__ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..2abcec7 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,145 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + exclude_types: [rst] + - id: fix-byte-order-marker + + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v3.3.1 + hooks: + - id: pyupgrade + args: [--py36-plus] + + # PyDocStyle + - repo: https://github.com/PyCQA/pydocstyle + rev: 6.3.0 + hooks: + - id: pydocstyle + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] + + - repo: https://github.com/pycqa/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + args: ["--extend-ignore=E501"] + + # Uncrustify + - repo: local + hooks: + - id: ament_uncrustify + name: ament_uncrustify + description: Uncrustify. + stages: [commit] + entry: ament_uncrustify + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--reformat"] + + # CPP hooks + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-14 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + + # - repo: local + # hooks: + # - id: ament_cppcheck + # name: ament_cppcheck + # description: Static code analysis of C/C++ files. + # stages: [commit] + # entry: ament_cppcheck + # language: system + # files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + # Maybe use https://github.com/cpplint/cpplint instead + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=100", "--filter=-whitespace/newline"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [commit] + entry: ament_lint_cmake + language: system + files: CMakeLists\.txt$ + + # Copyright + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + exclude: CHANGELOG\.rst$ + + - repo: https://github.com/pre-commit/pygrep-hooks + rev: v1.10.0 + hooks: + - id: rst-backticks + exclude: CHANGELOG\.rst$ + - id: rst-directive-colons + - id: rst-inline-touching-normal + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.2.2 + hooks: + - id: codespell + args: ['--write-changes'] + exclude: CHANGELOG\.rst|\.(svg|pyc)$ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..a0db6ba --- /dev/null +++ b/README.md @@ -0,0 +1,57 @@ +# needle_pantograph_v2 +Second year master project concerning the development of a haptic training device for needle insertion, based on the 2-Dof pantograph used at the ICube laboratory. + +Documentation: +[Report](doc/placeholder.txt) +[Videos](doc/videos) + +## Installation + +### Prepare the environment + +1) Install ROS2 Humble and the usual tools (Gazebo, Colcon, etc.) + +2) Install EtherLab as specified in the [documentation of ethercat_driver_ros2](https://icube-robotics.github.io/ethercat_driver_ros2/). + +> [!TIP] +> use the `ethercat slaves` CLI command to check that all is OK andf that you can see your device. + +### Install this package + +```bash +WS_PANTOGRAPH=~/dev/ws_pantograph_ros2/ +mkdir -p $WS_PANTOGRAPH/src +cd $WS_PANTOGRAPH/src + +git clone https://github.com/ICube-Robotics/needle_pantograph_ros2.git +vcs import . < needle_pantograph_ros2/needle_pantograph_ros2.repos +rosdep install --ignore-src --from-paths . -y -r + +cd .. +colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install +source install/setup.bash +``` + +## Getting started + +```bash +cd $WS_PANTOGRAPH +source install/setup.bash +``` + +```bash +# Launch with mock hardware +ros2 launch pantograph_bringup pantograph.launch.py +``` + +```bash +# With the actual robot +sudo /etc/init.d/ethercat start # start ETherLab daemon +ros2 launch pantograph_bringup pantograph.launch.py use_fake_hardware:=false +``` +```bash +# Load haptic_controller +ros2 control load_controller haptic_controller --set-state active +``` + +## TODO: finish readme \ No newline at end of file diff --git a/dicom_viewer/LICENSE b/dicom_viewer/LICENSE new file mode 100644 index 0000000..0136238 --- /dev/null +++ b/dicom_viewer/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2019 Wen-Ya Lin (林温雅) + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/dicom_viewer/dicom_viewer/__init__.py b/dicom_viewer/dicom_viewer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/dicom_viewer/dicom_viewer/mainwindow.py b/dicom_viewer/dicom_viewer/mainwindow.py new file mode 100644 index 0000000..ed3668e --- /dev/null +++ b/dicom_viewer/dicom_viewer/mainwindow.py @@ -0,0 +1,108 @@ +#!/usr/bin/env python3 + +# flake8: noqa: 501 +# flake8: noqa: 405 +import sys +import os +from PyQt5.QtCore import Qt +from PyQt5.QtGui import QPainter, QPixmap +from PyQt5.QtWidgets import QApplication, QMainWindow +from PyQt5.uic import loadUi + +from dicom_viewer.twoD.twoD_module import CtwoD +from dicom_viewer.threeD.threeD_module import CthreeD + +import rclpy + + +class CMainWindow(QMainWindow): + def __init__(self): + super().__init__() + dirname = os.path.dirname(__file__) + ui_path = os.path.join(dirname, 'mainwindow.ui') + loadUi(ui_path, self) + self.showMaximized() + self.twoDaction.changed.connect(self.activate_2d) + self.threeDaction.changed.connect(self.activate_3d) + self.maindirectory = os.getcwd() + print(self.maindirectory) + self.ui_2D = None + self.ui_3D = None + self.refresh2D = True + self.refresh3D = True + self.justclose = False + self.first2D = True + self.first3D = True + + def close2d(self): + os.chdir(self.maindirectory) + self.justclose = True + self.twoDaction.setChecked(False) + self.ui_2D.__init__() + self.refresh2D = True + self.justclose = False + + def close3d(self): + os.chdir(self.maindirectory) + self.justclose = True + self.threeDaction.setChecked(False) + self.ui_3D.__init__() + self.refresh3D = True + self.justclose = False + self.ui_3D.point_publisher_node.destroy_node() + rclpy.shutdown() + + def paintEvent(self, event): + super().paintEvent(event) + painter = QPainter(self) + # 設定透明度 + painter.setOpacity(0.5) + w, h = self.width(), self.height() + painter.drawPixmap(self.rect(), QPixmap('/home/telecom/dev/ws_pantograph_ros2/src/needle_pantograph_ros2/dicom_viewer/dicom_viewer/resources/background.png').scaled(w, h, Qt.KeepAspectRatio)) + self.label.setText('Dicom Viewer Project

' + '2D & 3D Processing Included') + + def activate_2d(self): + # setChecked會被視為一種toggled, rejected也會被視為一種toggled + os.chdir(self.maindirectory) + if self.refresh2D: + if self.first2D: + self.ui_2D = CtwoD() + self.first2D = False + self.ui_2D.show() + self.refresh2D = False + self.ui_2D.rejected.connect(self.close2d) + elif self.justclose: + pass + else: + if not self.twoDaction.isChecked(): + self.twoDaction.setChecked(True) + self.ui_2D.raise_() + + def activate_3d(self): + os.chdir(self.maindirectory) + if self.refresh3D: + if self.first3D: + self.ui_3D = CthreeD() + self.first3D = False + self.ui_3D.show() + self.refresh3D = False + self.ui_3D.rejected.connect(self.close3d) + elif self.justclose: + pass + else: + if not self.threeDaction.isChecked(): + self.threeDaction.setChecked(True) + self.ui_3D.raise_() + + +def main(args=None): + app = QApplication(sys.argv) # QApplication eats argv in constructor + window = CMainWindow() + window.setWindowTitle('Dicom Viewer Project') + window.show() + sys.exit(app.exec_()) + + +if __name__ == '__main__': + main() diff --git a/dicom_viewer/dicom_viewer/mainwindow.ui b/dicom_viewer/dicom_viewer/mainwindow.ui new file mode 100644 index 0000000..6c8b2c8 --- /dev/null +++ b/dicom_viewer/dicom_viewer/mainwindow.ui @@ -0,0 +1,165 @@ + + + MainWindow + + + + 0 + 0 + 1060 + 792 + + + + MainWindow + + + + + + + + + + + Futura + 48 + true + + + + QLabel{ + border: 2px solid green; + border-radius: 50px; + padding: 2px; + color: black; +} + + + + + + Qt::AlignCenter + + + + + + + + + 0 + 0 + 1060 + 22 + + + + + Options + + + + + + + + + + + YEEE + + + + + eee + + + + + ddd + + + + + ???? + + + + + sswe + + + + + sdfgsdg + + + + + ccc + + + + + true + + + false + + + true + + + 2D processing + + + false + + + true + + + QAction::TextHeuristicRole + + + true + + + + + true + + + false + + + true + + + 3D processing + + + false + + + + + true + + + Face Detection + + + + + true + + + Motion Detection + + + + + + diff --git a/dicom_viewer/dicom_viewer/pointPublisher.py b/dicom_viewer/dicom_viewer/pointPublisher.py new file mode 100644 index 0000000..bdf8a87 --- /dev/null +++ b/dicom_viewer/dicom_viewer/pointPublisher.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python3 +# flake8: noqa: 501 + +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float64MultiArray + + +class PointPublisher(Node): + # Publishes a point 3D position as a Float64MultiArray + + def __init__(self): + super().__init__('point_publisher') + self.publisher_ = self.create_publisher(Float64MultiArray, 'image_points', 10) + print('Point publisher initialized !') + # timer_period = 1 + # self.timer = self.create_timer(timer_period, self.point_publisher_callback) + + def publish_points(self, insertion_point, target_point): + msg = Float64MultiArray() + if len(insertion_point) == 0 or len(target_point) == 0: + msg.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + else: + # Insertion point + msg.data.append(insertion_point[0]) + msg.data.append(insertion_point[1]) + msg.data.append(insertion_point[2]) + + # Target point + msg.data.append(target_point[0]) + msg.data.append(target_point[1]) + msg.data.append(target_point[2]) + + # Publish the message + self.publisher_.publish(msg) + self.get_logger().info(f'Publishing insertion_point : {msg.data[0]:f}, {msg.data[1]:f}, {msg.data[2]:f}') + self.get_logger().info(f'Publishing target_point : {msg.data[3]:f}, {msg.data[4]:f}, {msg.data[5]:f}') + + +def main(args=None): + rclpy.init(args=args) + node = PointPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/dicom_viewer/dicom_viewer/requirements.txt b/dicom_viewer/dicom_viewer/requirements.txt new file mode 100644 index 0000000..118abdf --- /dev/null +++ b/dicom_viewer/dicom_viewer/requirements.txt @@ -0,0 +1,6 @@ +matplotlib==3.0.3 +numpy==1.15.4 +opencv-python==3.4.3.18 +packaging==18.0 +pydicom==1.2.0 +PyQt5==5.11.3 diff --git a/dicom_viewer/dicom_viewer/resources/background.png b/dicom_viewer/dicom_viewer/resources/background.png new file mode 100644 index 0000000..a407182 Binary files /dev/null and b/dicom_viewer/dicom_viewer/resources/background.png differ diff --git a/dicom_viewer/dicom_viewer/threeD/loaddicomfile.py b/dicom_viewer/dicom_viewer/threeD/loaddicomfile.py new file mode 100644 index 0000000..b76d994 --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/loaddicomfile.py @@ -0,0 +1,190 @@ +# flake8: noqa: 501 + +import numpy as np +import pydicom +import os +import scipy.ndimage +from skimage import measure +import glob + + +def load_dcm_info(path, private): + # 取第一個slice就好 + slice_for_info = pydicom.read_file(path + '/' + os.listdir(path)[0], force=True) + + if private: + name = ('Patient Name', 'Private') + _id = ('Patient ID', 'Private') + age = ('Patient Age', 'Private') + sex = ('Patient Sex', 'Private') + institution_name = ('Institution', 'Private') + date = ('Date', 'Private') + modality = ('Modality', 'Private') + manufacturer = ('Manufacturer', 'Private') + else: + try: + name = ('Patient Name', str(slice_for_info.PatientName).split(' ', 1)[1]) + except: + name = ('Patient Name', 'Anonymous') + try: + _id = ('Patient ID', str(slice_for_info.PatientID)) + except: + _id = ('Patient ID', 'Unknown') + try: + age = ('Patient Age', str(slice_for_info.PatientAge)) + except: + age = ('Patient Age', 'Unknown') + + try: + sex = ('Patient Sex', str(slice_for_info.PatientSex)) + except: + sex = ('Patient Sex', 'Unknown') + + try: + institution_name = ('Institution', str(slice_for_info.InstitutionName)) + except: + institution_name = ('Institution', 'Unknown') + + try: + date = ('Date', str(slice_for_info.InstanceCreationDate)) + except: + date = ('Date', 'Unknown') + + try: + modality = ('Modality', str(slice_for_info.Modality)) + except: + modality = ('Modality', 'Unknown') + + try: + manufacturer = ('Manufacturer', str(slice_for_info.Manufacturer)) + except: + manufacturer = ('Manufacturer', 'Unknown') + + # Test get DICOM metadata + + try: + IPP = ('Image Position Patient', str(slice_for_info.ImagePositionPatient)) + except: + IPP = ('Image Position Patient', 'Unknown') + + try: + IOP_x = ('Image Orientation Patient (x_axis)', str(slice_for_info.ImageOrientationPatient[0:3])) + except: + IOP_x = ('Image Orientation Patient (x_axis)', 'Unknown') + + try: + IOP_y = ('Image Orientation Patient (y_axis)', str(slice_for_info.ImageOrientationPatient[3:6])) + except: + IOP_y = ('Image Orientation Patient (y_axis)', 'Unknown') + + try: + slice_thickness = ('Slice thickness', str(slice_for_info.SliceThickness)) + except: + slice_thickness = ('Slice thickness', 'Unknown') + + try: + delta_i = ('Pixel spacing delta_i', str(slice_for_info.PixelSpacing[0])) + except: + delta_i = ('Pixel spacing delta_i', 'Unknown') + + try: + delta_j = ('Pixel spacing delta_j', str(slice_for_info.PixelSpacing[1])) + except: + delta_j = ('Pixel spacing delta_j', 'Unknown') + + info = [name, _id, age, sex, date, institution_name, modality, manufacturer, + IPP, IOP_x, IOP_y, slice_thickness, delta_i, delta_j] + return info + + +def load_scan(path): + f = glob.glob(rf'{path}/*.dcm') + slices = [pydicom.read_file(s, force=True) for s in f] + # slices = [pydicom.read_file(path + '/' + s, force=True) for s in os.listdir(path)] + all_metadata = [] + for s in slices: + s.file_meta.TransferSyntaxUID = pydicom.uid.ImplicitVRLittleEndian + slices.sort(key=lambda x: int(x.InstanceNumber)) + # thickness在dicom裡面是空白value，算一下給它值（兩個slice間的z差值） + try: + slice_thickness = np.abs(slices[0].ImagePositionPatient[2] - slices[1].ImagePositionPatient[2]) + except Exception: + slice_thickness = np.abs(slices[0].SliceLocation - slices[1].SliceLocation) + for s in slices: + s.SliceThickness = slice_thickness + + # Test get DICOM metadata + slice_metadata = get_metadata(s) + all_metadata.append(slice_metadata) + + # orientation = slices[0].ImageOrientationPatient + # print(orientation) + + return slices, all_metadata + + +def get_pixels_hu(scans): + image = np.flipud(np.stack([s.pixel_array for s in scans])) + # Convert to int16 (from sometimes int16), + # should be possible as values should always be low enough (<32k) + image = image.astype(np.int16) + + # Set outside-of-scan pixels to 1 + # The intercept is usually -1024, so air is approximately 0 + # 在影像中非人體部位（周遭空氣等）被拍到的地方數值會是-2000，把那些設為0 + image[image == -2000] = 0 + + # Convert to Hounsfield units (HU) + intercept = scans[0].RescaleIntercept + slope = scans[0].RescaleSlope + + if slope != 1: + image = slope * image.astype(np.float64) + image = image.astype(np.int16) + + image += np.int16(intercept) + return np.array(image, dtype=np.int16) + + +def resample(image, scan): + # Determine current pixel spacing + new_spacing = [1, 1, 1] + spacing = map(float, ([scan[0].SliceThickness] + [scan[0].PixelSpacing[0]] + [scan[0].PixelSpacing[1]])) + spacing = np.asarray(list(spacing)) + + resize_factor = spacing / new_spacing + new_real_shape = image.shape * resize_factor + new_shape = np.round(new_real_shape) + real_resize_factor = new_shape / image.shape + new_spacing = spacing / real_resize_factor + image = scipy.ndimage.interpolation.zoom(image, real_resize_factor) + + return image, new_spacing + + +def make_mesh(image, threshold=-300, step_size=10): + print('Transposing surface') + # 不同的transpose，圖的方位不一樣，遵循convention以(2,1,0)做 + p = image.transpose(2, 1, 0) + print('Calculating surface') + # verts是所有頂點，faces是所有三角形列表 + verts, faces, norm, val = measure.marching_cubes_lewiner(p, threshold, spacing=(1, 1, 1), + gradient_direction='descent', step_size=step_size, + allow_degenerate=True) + return verts, faces + + +def get_metadata(ds): + # Extract relevant metadata + metadata = { + # 'PatientName': ds.PatientName, + # 'StudyDescription': ds.StudyDescription, + # 'Rows': ds.Rows, + # 'Columns': ds.Columns, + 'PixelSpacing': ds.PixelSpacing, + 'ImageOrientationPatient': ds.ImageOrientationPatient, + 'ImagePositionPatient': ds.ImagePositionPatient, + 'SliceThickness': ds.SliceThickness, + # Add more fields as needed + } + return metadata diff --git a/dicom_viewer/dicom_viewer/threeD/matinqt.py b/dicom_viewer/dicom_viewer/threeD/matinqt.py new file mode 100644 index 0000000..e4d6a59 --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/matinqt.py @@ -0,0 +1,25 @@ +from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas +from matplotlib.figure import Figure +from mpl_toolkits.mplot3d.art3d import Poly3DCollection + + +class CFigureCanvas(FigureCanvas): + + def __init__(self, parent=None): + # 直接不要給figsize就會自己fit了 + fig = Figure() + FigureCanvas.__init__(self, fig) + self.setParent(parent) + self.axes = fig.add_subplot(111, projection='3d') + + def plt_3d(self, verts, faces, alpha=0.5): + print("Drawing") + x, y, z = zip(*verts) + mesh = Poly3DCollection(verts[faces], linewidths=0.05, alpha=alpha) + face_color = [1, 1, 0.9] + mesh.set_facecolor(face_color) + self.axes.add_collection3d(mesh) + self.axes.set_xlim(0, max(x)) + self.axes.set_ylim(0, max(y)) + self.axes.set_zlim(0, max(z)) + self.axes.set_facecolor((0.7, 0.7, 0.7)) diff --git a/dicom_viewer/dicom_viewer/threeD/qpaintlabel3.py b/dicom_viewer/dicom_viewer/threeD/qpaintlabel3.py new file mode 100644 index 0000000..090d6e0 --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/qpaintlabel3.py @@ -0,0 +1,154 @@ +# flake8: noqa + +from PyQt5.QtWidgets import * +from PyQt5.QtGui import * +from PyQt5 import QtCore +from PyQt5.QtCore import * +import numpy as np +# from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas + + +class QPaintLabel3(QLabel): + + coords_updated = pyqtSignal(list) # Signal to emit when coords are updated + mpsignal = pyqtSignal(str) + + def __init__(self, parent): + super(QLabel, self).__init__(parent) + + self.setMinimumSize(1, 1) + self.setMouseTracking(False) + self.image = None + self.processedImage = None + self.imgr, self.imgc = None, None + self.imgpos_x, self.imgpos_y = None, None + self.pos_x = 20 + self.pos_y = 20 + self.imgr, self.imgc = None, None + # 遇到list就停，圖上的顯示白色只是幌子 + self.pos_xy = [] + # 十字的中心點！每個QLabel指定不同中心點，這樣可以用一樣的paintevent function + self.crosscenter = [0, 0] + self.mouseclicked = None + self.sliceclick = False + # 決定用哪種paintEvent的type, general代表一般的 + self.type = 'general' + self.slice_loc = [0, 0, 0] + self.slice_loc_restore = [0, 0, 0] + self.mousein = False + + def mouseMoveEvent(self, event: QMouseEvent): + super().mouseMoveEvent(event) + + if not self.mousein: + self.slice_loc_restore = self.slice_loc.copy() + self.mousein = True + + self.imgpos_x = int(event.x() * self.imgc / self.width()) + self.imgpos_y = int(event.y() * self.imgr / self.height()) + + if self.type == 'axial': + self.slice_loc[0:2] = self.imgpos_x, self.imgpos_y + elif self.type == 'sagittal': + self.slice_loc[1:3] = self.imgpos_x, self.imgr - self.imgpos_y + elif self.type == 'coronal': + self.slice_loc[0] = self.imgpos_x + self.slice_loc[2] = self.imgr - self.imgpos_y + else: + pass + self.update() + + def leaveEvent(self, event): + self.mousein = False + self.slice_loc = self.slice_loc_restore + self.update() + + def mousePressEvent(self, event: QMouseEvent): + self.crosscenter[0] = event.x() + self.crosscenter[1] = event.y() + + # Test emit mouse coords + self.mpsignal.emit(self.type) + self.coords_updated.emit(self.slice_loc) + + self.slice_loc_restore = self.slice_loc.copy() + self.update() + + def display_image(self, window=1): + self.imgr, self.imgc = self.processedImage.shape[0:2] + qformat = QImage.Format_Indexed8 + if len(self.processedImage.shape) == 3: # rows[0], cols[1], channels[2] + if (self.processedImage.shape[2]) == 4: + qformat = QImage.Format_RGBA8888 + else: + qformat = QImage.Format_RGB888 + img = QImage(self.processedImage, self.processedImage.shape[1], self.processedImage.shape[0], + self.processedImage.strides[0], qformat) + img = img.rgbSwapped() + w, h = self.width(), self.height() + if window == 1: + self.setScaledContents(True) + backlash = self.lineWidth() * 2 + self.setPixmap(QPixmap.fromImage(img).scaled(w - backlash, h - backlash, Qt.IgnoreAspectRatio)) + self.setAlignment(QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter) + self.update() + + def paintEvent(self, event): + super().paintEvent(event) + # 利用一個QFont來設定drawText的格式 + loc = QFont() + loc.setPixelSize(10) + loc.setBold(True) + loc.setItalic(True) + loc.setPointSize(15) + if self.pixmap(): + painter = QPainter(self) + pixmap = self.pixmap() + painter.drawPixmap(self.rect(), pixmap) + + painter.setPen(QPen(Qt.magenta, 10)) + painter.setFont(loc) + painter.drawText(5, self.height() - 5, 'x = %3d , y = %3d , z = %3d' + % (self.slice_loc[0], self.slice_loc[1], self.slice_loc[2])) + + if self.type == 'axial': + # 畫直條 + painter.setPen(QPen(Qt.red, 3)) + painter.drawLine(self.crosscenter[0], 0, self.crosscenter[0], self.height()) + # 畫橫條 + painter.setPen(QPen(Qt.cyan, 3)) + painter.drawLine(0, self.crosscenter[1], self.width(), self.crosscenter[1]) + # 畫中心 + painter.setPen(QPen(Qt.yellow, 3)) + painter.drawPoint(self.crosscenter[0], self.crosscenter[1]) + + elif self.type == 'sagittal': + # 畫直條 + painter.setPen(QPen(Qt.cyan, 3)) + painter.drawLine(self.crosscenter[0], 0, self.crosscenter[0], self.height()) + # 畫橫條 + painter.setPen(QPen(Qt.yellow, 3)) + painter.drawLine(0, self.crosscenter[1], self.width(), self.crosscenter[1]) + # 畫中心 + painter.setPen(QPen(Qt.red, 3)) + painter.drawPoint(self.crosscenter[0], self.crosscenter[1]) + + elif self.type == 'coronal': + # 畫直條 + painter.setPen(QPen(Qt.red, 3)) + painter.drawLine(self.crosscenter[0], 0, self.crosscenter[0], self.height()) + # 畫橫條 + painter.setPen(QPen(Qt.yellow, 3)) + painter.drawLine(0, self.crosscenter[1], self.width(), self.crosscenter[1]) + # 畫中心 + painter.setPen(QPen(Qt.cyan, 3)) + painter.drawPoint(self.crosscenter[0], self.crosscenter[1]) + + else: + pass + + +def linear_convert(img): + convert_scale = 255.0 / (np.max(img) - np.min(img)) + converted_img = convert_scale*img-(convert_scale*np.min(img)) + return converted_img diff --git a/dicom_viewer/dicom_viewer/threeD/resources/double-arrow-down.png b/dicom_viewer/dicom_viewer/threeD/resources/double-arrow-down.png new file mode 100644 index 0000000..5a4e962 Binary files /dev/null and b/dicom_viewer/dicom_viewer/threeD/resources/double-arrow-down.png differ diff --git a/dicom_viewer/dicom_viewer/threeD/resources/down_arrow.png b/dicom_viewer/dicom_viewer/threeD/resources/down_arrow.png new file mode 100644 index 0000000..dc308fb Binary files /dev/null and b/dicom_viewer/dicom_viewer/threeD/resources/down_arrow.png differ diff --git a/dicom_viewer/dicom_viewer/threeD/threeD_module.py b/dicom_viewer/dicom_viewer/threeD/threeD_module.py new file mode 100644 index 0000000..e243da3 --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/threeD_module.py @@ -0,0 +1,412 @@ +# flake8: noqa: 501 + +import sys +from PyQt5.QtWidgets import * +from PyQt5.uic import loadUi +import os +import cv2 +import dicom_viewer.threeD.loaddicomfile as ldf +import numpy as np +from scipy.spatial.transform import Rotation as R +from dicom_viewer.threeD.vol_view_module import C3dView +from PyQt5.QtCore import pyqtSlot +import rclpy +from dicom_viewer.pointPublisher import PointPublisher + +# adding folder dicom_viewer to the system path +sys.path.insert(0, '/home/telecom/dev/ws_pantograph_ros2/src/needle_pantograph_ros2/dicom_viewer') + + +class CthreeD(QDialog): + def __init__(self): + super().__init__() + # path = os.getcwd() + path = '/home/telecom/dev/ws_pantograph_ros2/src/needle_pantograph_ros2/dicom_viewer/dicom_viewer/threeD' + os.chdir(path) + self.directory = os.getcwd() + loadUi('threeD_module.ui', self) + self.setWindowTitle('3D Processing') + self.image = None + self.voxel = None + self.processedvoxel = None + self.v1, self.v2, self.v3 = None, None, None + self.volWindow = None + self.dicomButton.clicked.connect(self.dicom_clicked) + self.axial_hSlider.valueChanged.connect(self.updateimg) + self.axial_vSlider.valueChanged.connect(self.updateimg) + self.sagittal_hSlider.valueChanged.connect(self.updateimg) + self.sagittal_vSlider.valueChanged.connect(self.updateimg) + self.coronal_hSlider.valueChanged.connect(self.updateimg) + self.coronal_vSlider.valueChanged.connect(self.updateimg) + self.colormap = None + # 這樣可以把"被activate"的Item轉成str傳入connect的function（也可以用int之類的，會被enum） + self.colormapBox.activated[str].connect(self.colormap_choice) + self.colormapDict = {'GRAY': None, + 'AUTUMN': cv2.COLORMAP_AUTUMN, + 'BONE': cv2.COLORMAP_BONE, + 'COOL': cv2.COLORMAP_COOL, + 'HOT': cv2.COLORMAP_HOT, + 'HSV': cv2.COLORMAP_HSV, + 'JET': cv2.COLORMAP_JET, + 'OCEAN': cv2.COLORMAP_OCEAN, + 'PINK': cv2.COLORMAP_PINK, + 'RAINBOW': cv2.COLORMAP_RAINBOW, + 'SPRING': cv2.COLORMAP_SPRING, + 'SUMMER': cv2.COLORMAP_SUMMER, + 'WINTER': cv2.COLORMAP_WINTER + } + self.volButton.clicked.connect(self.open_3dview) + + self.w, self.h = self.imgLabel_1.width(), self.imgLabel_1.height() + + self.imgLabel_1.type = 'axial' + self.imgLabel_2.type = 'sagittal' + self.imgLabel_3.type = 'coronal' + + self.axialGrid.setSpacing(0) + self.saggitalGrid.setSpacing(0) + self.coronalGrid.setSpacing(0) + + h = QSpacerItem(10, 10, QSizePolicy.Fixed, QSizePolicy.Fixed) + v = QSpacerItem(10, 10, QSizePolicy.Fixed, QSizePolicy.Fixed) + self.axial_vBox.setSpacing(0) + self.axial_vBox.insertSpacerItem(0, v) + self.axial_vBox.insertSpacerItem(2, v) + self.axial_hBox.setSpacing(0) + self.axial_hBox.insertSpacerItem(0, h) + self.axial_hBox.insertSpacerItem(2, h) + self.saggital_vBox.setSpacing(0) + self.saggital_vBox.insertSpacerItem(0, v) + self.saggital_vBox.insertSpacerItem(2, v) + self.saggital_hBox.setSpacing(0) + self.saggital_hBox.insertSpacerItem(0, h) + self.saggital_hBox.insertSpacerItem(2, h) + self.coronal_vBox.setSpacing(0) + self.coronal_vBox.insertSpacerItem(0, v) + self.coronal_vBox.insertSpacerItem(2, v) + self.coronal_hBox.setSpacing(0) + self.coronal_hBox.insertSpacerItem(0, h) + self.coronal_hBox.insertSpacerItem(2, h) + + self.colormap_hBox.insertStretch(2) + self.colormap_hBox.insertSpacerItem(0, QSpacerItem(30, 0, QSizePolicy.Fixed, QSizePolicy.Fixed)) + + self.savesliceButton.clicked.connect(self.saveslice_clicked) + self.dcmInfo = None + self.imgLabel_1.mpsignal.connect(self.cross_center_mouse) + self.imgLabel_2.mpsignal.connect(self.cross_center_mouse) + self.imgLabel_3.mpsignal.connect(self.cross_center_mouse) + + self.cross_recalc = True + self.savenpyButton.clicked.connect(self.save_npy_clicked) + self.loadnpyButton.clicked.connect(self.load_npy_clicked) + self.downscaled = 2 + self.dsampleButton.clicked.connect(self.downsample) + + # Get mouse coords from axial view + self.imgLabel_1.coords_updated.connect(self.handle_mouse_coords_update) + + # Get slices metadata + self.all_metadata = [] + + # Button setup for choosing insertion and target point + self.SetInsertionPointButton.clicked.connect(self.set_insertion_pt_clicked) + self.SetTargetPointButton.clicked.connect(self.set_target_pt_clicked) + + # Insertion and target point coords + self.insertion_point = [0.0, 0.0, 0.0] + self.target_point = [0.0, 0.0, 0.0] + + self.T_WP = np.eye(4) + self.T_PI = np.eye(4) + + # Scale factor (testing only) + self.scale_factor = 0.001 + + # Test ROS2 publisher for point coords + self.insertion_point_msg = [0.0, 0.0, 0.0] + self.target_point_msg = [0.0, 0.0, 0.0] + + rclpy.init(args=None) + self.point_publisher_node = PointPublisher() + print('Point publisher created') + + def downsample(self): + self.processedvoxel = self.processedvoxel[::self.downscaled, ::self.downscaled, ::self.downscaled] + self.update_shape() + self.updateimg() + + def save_npy_clicked(self): + fname, _filter = QFileDialog.getSaveFileName(self, 'save file', '~/untitled', "Image Files (*.npy)") + if fname: + np.save(fname, self.processedvoxel) + else: + print('Error') + + def load_npy_clicked(self): + fname, _filter = QFileDialog.getOpenFileName(self, 'open file', '~/Desktop', "Image Files (*.NPY *.npy)") + self.processedvoxel = np.load(fname) + self.update_shape() + self.savetemp() + self.updateimg() + + def set_directory(self): + os.chdir(self.directory) + + def cross_center_mouse(self, _type): + self.cross_recalc = False + if _type == 'axial': + self.axial_hSlider.setValue(int(self.imgLabel_1.crosscenter[0] * + self.axial_hSlider.maximum() / self.imgLabel_1.width())) + self.axial_vSlider.setValue(int(self.imgLabel_1.crosscenter[1] * + self.axial_vSlider.maximum() / self.imgLabel_1.height())) + elif _type == 'sagittal': + self.sagittal_hSlider.setValue(int(self.imgLabel_2.crosscenter[0] * + self.sagittal_hSlider.maximum() / self.imgLabel_2.width())) + self.sagittal_vSlider.setValue(int(self.imgLabel_2.crosscenter[1] * + self.sagittal_vSlider.maximum() / self.imgLabel_2.height())) + elif _type == 'coronal': + self.coronal_hSlider.setValue(int(self.imgLabel_3.crosscenter[0] * + self.coronal_hSlider.maximum() / self.imgLabel_3.width())) + self.coronal_vSlider.setValue(int(self.imgLabel_3.crosscenter[1] * + self.coronal_vSlider.maximum() / self.imgLabel_3.height())) + else: + pass + + self.imgLabel_1.crosscenter = [ + int(self.axial_hSlider.value() * self.imgLabel_1.width() / self.axial_hSlider.maximum()), + int(self.axial_vSlider.value() * self.imgLabel_1.height() / self.axial_vSlider.maximum())] + self.imgLabel_2.crosscenter = [ + int(self.sagittal_hSlider.value() * self.imgLabel_2.width() / self.sagittal_hSlider.maximum()), + int(self.sagittal_vSlider.value() * self.imgLabel_2.height() / self.sagittal_vSlider.maximum())] + self.imgLabel_3.crosscenter = [ + int(self.coronal_hSlider.value() * self.imgLabel_3.width() / self.coronal_hSlider.maximum()), + int(self.coronal_vSlider.value() * self.imgLabel_3.height() / self.coronal_vSlider.maximum())] + self.updateimg() + + self.cross_recalc = True + + def saveslice_clicked(self): + fname, _filter = QFileDialog.getSaveFileName(self, 'save file', '~/untitled', "Image Files (*.jpg)") + if fname: + if self.savesliceBox.currentText() == 'Axial': + cv2.imwrite(fname, self.imgLabel_1.processedImage) + elif self.savesliceBox.currentText() == 'Saggital': + cv2.imwrite(fname, self.imgLabel_2.processedImage) + elif self.savesliceBox.currentText() == 'Coronal': + cv2.imwrite(fname, self.imgLabel_3.processedImage) + else: + print('No slice be chosen') + else: + print('Error') + pass + + def resizeEvent(self, event): + super().resizeEvent(event) + self.w = self.imgLabel_1.width() + self.h = self.imgLabel_1.height() + if self.processedvoxel is not None: + self.updateimg() + + def open_3dview(self): + self.volWindow.setWindowTitle('3D View') + self.volWindow.vol_show() + self.volWindow.show() + + def colormap_choice(self, text): + self.colormap = self.colormapDict[text] + self.updateimg() + + def dicom_clicked(self): + dname = QFileDialog.getExistingDirectory(self, 'choose dicom directory') + print(dname) + self.load_dicomfile(dname) + + def set_insertion_pt_clicked(self): + self.insertion_point = self.imgLabel_1.slice_loc + print('Insertion point coords in image frame : [x, y, z] \n', self.insertion_point) + + # Get slice metadata + current_slice_number = self.insertion_point[2] + slice_metadata = self.all_metadata[current_slice_number] + # print('Slice metadata : \n', self.all_metadata[coords[2]]) + + # Convert pixel coords to homogeneous pixel coords + pixel_coords = [self.insertion_point[0], self.insertion_point[1], 0, 1] + + # Insertion point coords in patient frame (in mm) + patient_insertion_point = self.image_to_patient(pixel_coords, slice_metadata) + + # Apply scale factor to patient coords to transform from mm to m + patient_insertion_point[:3] = self.scale_factor * patient_insertion_point[:3] + + # Robot insertion point coords in world frame (in m) + robot_insertion_point = [0.0, 0.16056, 0.09] + + # Compute translation from world frame to patient frame (in m) + # Needed to overlap patient_insertion_point and robot_insertion_point + t_WP = robot_insertion_point - patient_insertion_point[:3] + + # Compute rotation needed to orientate image as desired + # R_WP = np.eye(3) + R_WP = R.from_euler('xyz', [180, 0, -90], degrees=True).as_matrix() + + # Compute hogeneous transformation matrix from world frame to patient frame + # offset = [-0.002719, 0.0, 0.0] # offset for testing only + self.T_WP = np.eye(4) + self.T_WP[:3, :3] = R_WP + self.T_WP[:3, 3] = t_WP # + offset + + # Verify the transformation + # T_WP in m T_PI in mm + p1 = np.matmul(self.T_PI, pixel_coords).ravel() + p1[:3] = self.scale_factor * p1[:3] # transform coords in mm to m + self.insertion_point_msg = (np.matmul(self.T_WP, p1).ravel()) + # print('Insertion point updated : \n', self.insertion_point_msg[:3]) + + # Publish coords to ROS2 topic + self.point_publisher_node.publish_points(self.insertion_point_msg[:3], self.insertion_point_msg[:3]) + + # TODO Draw point marker at insertion point + + def set_target_pt_clicked(self): + self.target_point = self.imgLabel_1.slice_loc + print('Target point pixel coords in image frame : [x, y, z] \n', self.target_point) + + # Get slice metadata + current_slice_number = self.target_point[2] + slice_metadata = self.all_metadata[current_slice_number] + # print('Slice metadata : \n', self.all_metadata[coords[2]]) + + # Transformation from image to world frame + # Convert pixel coords to homogeneous pixel coords + pixel_coords = [self.target_point[0], self.target_point[1], 0, 1] + + # Target point coords in patient frame (in mm) + patient_target_point = self.image_to_patient(pixel_coords, slice_metadata) + + # Apply scale factor to patient coords to transform from mm to m + patient_target_point[:3] = self.scale_factor * patient_target_point[:3] + + # Target point coords in world frame (in m) + world_target_point = np.matmul(self.T_WP, patient_target_point).ravel() + + self.target_point_msg = world_target_point + print('Target point 3D coords in robot world frame : [x,y,z] \n', self.target_point_msg[:3]) + + # Publish coords to ROS2 topic + self.point_publisher_node.publish_points(self.insertion_point_msg[:3], self.target_point_msg[:3]) + + def load_dicomfile(self, dname): + self.dcmList.clear() + patient, self.all_metadata = ldf.load_scan(dname) + imgs = ldf.get_pixels_hu(patient) + self.voxel = self.linear_convert(imgs) + self.processedvoxel = self.voxel.copy() + + self.update_shape() + + self.imgLabel_1.setMouseTracking(True) + self.imgLabel_2.setMouseTracking(True) + self.imgLabel_3.setMouseTracking(True) + + self.updateimg() + self.set_directory() + self.volWindow = C3dView() + self.volWindow.imgs = imgs + self.volWindow.patient = patient + self.dcmInfo = ldf.load_dcm_info(dname, self.privatecheckBox.isChecked()) + self.updatelist() + + def update_shape(self): + self.v1, self.v2, self.v3 = self.processedvoxel.shape + self.sagittal_vSlider.setMaximum(self.v1-1) + self.coronal_vSlider.setMaximum(self.v1-1) + self.sagittal_hSlider.setMaximum(self.v2-1) + self.axial_vSlider.setMaximum(self.v2-1) + self.coronal_hSlider.setMaximum(self.v3-1) + self.axial_hSlider.setMaximum(self.v3-1) + self.sagittal_vSlider.setValue(self.sagittal_vSlider.maximum()//2) + self.coronal_vSlider.setValue(self.coronal_vSlider.maximum()//2) + self.sagittal_hSlider.setValue(self.sagittal_hSlider.maximum()//2) + self.axial_vSlider.setValue(self.axial_vSlider.maximum()//2) + self.coronal_hSlider.setValue(self.coronal_hSlider.maximum()//2) + self.axial_hSlider.setValue(self.axial_hSlider.maximum()//2) + + def updatelist(self): + for item in self.dcmInfo: + # 單純字串的話，可以不需要QListWidgetItem包裝也沒關係 + self.dcmList.addItem(QListWidgetItem('%-20s\t: %s' % (item[0], item[1]))) + + def updateimg(self): + a_loc = self.sagittal_vSlider.value() + c_loc = self.axial_vSlider.value() + s_loc = self.axial_hSlider.value() + + axial = (self.processedvoxel[a_loc, :, :]).astype(np.uint8).copy() + sagittal = (self.processedvoxel[:, :, s_loc]).astype(np.uint8).copy() + coronal = (self.processedvoxel[:, c_loc, :]).astype(np.uint8).copy() + + self.imgLabel_1.slice_loc = [s_loc, c_loc, a_loc] + self.imgLabel_2.slice_loc = [s_loc, c_loc, a_loc] + self.imgLabel_3.slice_loc = [s_loc, c_loc, a_loc] + + if self.cross_recalc: + self.imgLabel_1.crosscenter = [self.w*s_loc//self.v3, self.h*c_loc//self.v2] + self.imgLabel_2.crosscenter = [self.w*c_loc//self.v2, self.h*a_loc//self.v1] + self.imgLabel_3.crosscenter = [self.w*s_loc//self.v3, self.h*a_loc//self.v1] + + if self.colormap is None: + self.imgLabel_1.processedImage = axial + self.imgLabel_2.processedImage = sagittal + self.imgLabel_3.processedImage = coronal + else: + self.imgLabel_1.processedImage = cv2.applyColorMap(axial, self.colormap) + self.imgLabel_2.processedImage = cv2.applyColorMap(sagittal, self.colormap) + self.imgLabel_3.processedImage = cv2.applyColorMap(coronal, self.colormap) + + self.imgLabel_1.display_image(1) + self.imgLabel_2.display_image(1) + self.imgLabel_3.display_image(1) + + def image_to_patient(self, pixel_coords, slice_metadata): + # Get metadata of the current image + # Origin of the current image (in Patient frame) + IPP = slice_metadata['ImagePositionPatient'] + # Orientation of the current image (in Patient frame) + IOP = slice_metadata['ImageOrientationPatient'] + # Pixel spacing values + deltai = slice_metadata['PixelSpacing'][0] + deltaj = slice_metadata['PixelSpacing'][1] + + # Transformation matrix from 2D pixels to 3D voxel in base frame + C1 = deltai * np.array([IOP[0], IOP[1], IOP[2], 0]) # direction cosines of image x axis + C2 = deltaj * np.array([IOP[3], IOP[4], IOP[5], 0]) # direction cosines of image y axis + C3 = np.array([0, 0, 0, 0]) + C4 = np.array([IPP[0], IPP[1], IPP[2], 1]) # Position of the image origin in patient frame + self.T_PI = np.transpose(np.stack((C1, C2, C3, C4))) # Transformation matrix from Patient to image frame + # print('transformation matrix : '+'\n',M) + + # Get homogeneous coords in base frame (in mm) + patient_coords = np.matmul(self.T_PI, pixel_coords).ravel() + # Convert to 3D coords + # space_coords = space_coords[:3] + return patient_coords + + @pyqtSlot(list) + def handle_mouse_coords_update(self, coords): + self.updateimg() + return + + @staticmethod + def linear_convert(img): + convert_scale = 255.0 / (np.max(img) - np.min(img)) + converted_img = convert_scale * img - (convert_scale * np.min(img)) + return converted_img + + +if __name__ == '__main__': + app = QApplication(sys.argv) + ex = CthreeD() + ex.show() + sys.exit(app.exec_()) diff --git a/dicom_viewer/dicom_viewer/threeD/threeD_module.ui b/dicom_viewer/dicom_viewer/threeD/threeD_module.ui new file mode 100644 index 0000000..2af42de --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/threeD_module.ui @@ -0,0 +1,1073 @@ + + + Dialog + + + Qt::NonModal + + + + 0 + 0 + 1140 + 585 + + + + + 995 + 0 + + + + Dialog + + + + + + + + + + 7 + + + + + + + + 0 + 0 + + + + + 1 + 1 + + + + CrossCursor + + + QFrame::Panel + + + QFrame::Plain + + + + + + Qt::AlignCenter + + + + + + + + + + + true + + + + 0 + 0 + + + + + 0 + 1 + + + + SplitVCursor + + + Qt::NoFocus + + + Qt::LeftToRight + + + 255 + + + 9 + + + 128 + + + 128 + + + true + + + Qt::Vertical + + + true + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + SplitHCursor + + + Qt::NoFocus + + + + + + 255 + + + 128 + + + true + + + Qt::Horizontal + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + + 0 + 0 + + + + Axial (Transpose) + + + Qt::AlignCenter + + + + + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + SplitHCursor + + + Qt::NoFocus + + + + + + 255 + + + 128 + + + true + + + Qt::Horizontal + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + true + + + + 0 + 0 + + + + + 0 + 1 + + + + SplitVCursor + + + Qt::NoFocus + + + Qt::LeftToRight + + + 255 + + + 9 + + + 128 + + + 128 + + + true + + + Qt::Vertical + + + true + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + + 0 + 0 + + + + Sagittal + + + Qt::AlignCenter + + + + + + + + + + + + 0 + 0 + + + + + 1 + 1 + + + + CrossCursor + + + QFrame::Panel + + + QFrame::Plain + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + SplitHCursor + + + Qt::NoFocus + + + + + + 255 + + + 128 + + + true + + + Qt::Horizontal + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + true + + + + 0 + 0 + + + + + 0 + 1 + + + + SplitVCursor + + + Qt::NoFocus + + + Qt::LeftToRight + + + 255 + + + 9 + + + 128 + + + 128 + + + true + + + Qt::Vertical + + + true + + + false + + + QSlider::TicksBelow + + + 1 + + + + + + + + 0 + 0 + + + + Coronal + + + Qt::AlignCenter + + + + + + + + + + + + 0 + 0 + + + + + 1 + 1 + + + + CrossCursor + + + QFrame::Panel + + + QFrame::Plain + + + + + + + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 186 + 0 + + + + PointingHandCursor + + + Qt::NoFocus + + + QPushButton{ + background-color: red; + border-style: outset; + border-width: 2px; + border-radius: 10px; + border-color: beige; + font: bold 14px; + min-width: 10em; + padding: 6px; +} + + + + Load DICOM File + + + + + + + + 0 + 0 + + + + + 186 + 0 + + + + PointingHandCursor + + + Qt::NoFocus + + + QPushButton{ + background-color: DodgerBlue; + border-style: outset; + border-width: 2px; + border-radius: 10px; + border-color: beige; + font: bold 14px; + min-width: 10em; + padding: 6px; +} + + + + Open 3D View + + + + + + + + + PointingHandCursor + + + Qt::NoFocus + + + QPushButton{ + background-color: #FFEFD5; + border-style: outset; + border-width: 2px; + border-radius: 10px; + border-color: #CD853F; + font: bold 14px; + min-width: 10em; + padding: 6px; +} + + + + Save Slice + + + + + + + + 0 + 0 + + + + PointingHandCursor + + + /* +QComboBox { +background: #7FFFD4; +border: 2px solid #8A2BE2; +border-radius: 10px; +margin: 0px; +} +*/ +QComboBox { + border: 2px solid #8A2BE2; + border-radius: 10px; + padding: 1px 10px 1px 20px; + min-width: 6em; +} +/* +QComboBox:editable { + background: red; +} +*/ +QComboBox:!editable, QComboBox::drop-down:editable { + background: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, + stop: 0 #E1E1E1, stop: 0.4 #DDDDDD, + stop: 0.5 #D8D8D8, stop: 1.0 #D3D3D3); +} + +QComboBox:!editable:on, QComboBox::drop-down:editable:on { + background: qlineargradient(x1: 0, y1: 0, x2: 0, y2: 1, + stop: 0 #D3D3D3, stop: 0.4 #D8D8D8, + stop: 0.5 #DDDDDD, stop: 1.0 #E1E1E1); +} + +QComboBox:on { /* shift the text when the popup opens */ + padding-top: 30px; + padding-left: 40px; +} + +QComboBox::drop-down { + subcontrol-origin: padding; + subcontrol-position: top right; + width: 20px; + + border-left-width: 1px; + border-left-color: darkgray; + border-left-style: solid; + border-top-right-radius: 10px; + border-bottom-right-radius: 10px; +} + +QComboBox::down-arrow { + image: url(resources/double-arrow-down.png); + width: 15px; + height: 15px; +} + + + + Axial + + + + + Sagittal + + + + + Coronal + + + + + + + + + + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + Dicom Info + + + + + + + + 0 + 0 + + + + Protected Display + + + + + + + + + + 0 + 0 + + + + + 350 + 100 + + + + + 800 + 100 + + + + + + + + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + ColorMap + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 1 + 0 + + + + PointingHandCursor + + + Qt::NoFocus + + + + GRAY + + + + + AUTUMN + + + + + BONE + + + + + COOL + + + + + HOT + + + + + HSV + + + + + JET + + + + + OCEAN + + + + + PINK + + + + + RAINBOW + + + + + SPRING + + + + + SUMMER + + + + + WINTER + + + + + + + + + 0 + 0 + + + + Set insertion point + + + false + + + + + + + + 0 + 0 + + + + Set target point + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + down sample + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Load .npy + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Save as .npy + + + + + + + + + + QPaintLabel3 + QLabel +
qpaintlabel3.h
+ + + + + + axial_hSlider + valueChanged(int) + coronal_hSlider + setValue(int) + + + 273 + 226 + + + 908 + 217 + + + + + coronal_hSlider + valueChanged(int) + axial_hSlider + setValue(int) + + + 908 + 217 + + + 234 + 212 + + + + + axial_vSlider + valueChanged(int) + sagittal_hSlider + setValue(int) + + + 28 + 344 + + + 479 + 215 + + + + + sagittal_hSlider + valueChanged(int) + axial_vSlider + setValue(int) + + + 488 + 219 + + + 20 + 249 + + + + + sagittal_vSlider + valueChanged(int) + coronal_vSlider + setValue(int) + + + 388 + 364 + + + 762 + 368 + + + + + coronal_vSlider + valueChanged(int) + sagittal_vSlider + setValue(int) + + + 762 + 294 + + + 388 + 307 + + + + + diff --git a/dicom_viewer/dicom_viewer/threeD/vol_view_module.py b/dicom_viewer/dicom_viewer/threeD/vol_view_module.py new file mode 100644 index 0000000..5f8b6ec --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/vol_view_module.py @@ -0,0 +1,56 @@ +import sys +from PyQt5.QtWidgets import QDialog, QGraphicsScene, QApplication +from PyQt5.uic import loadUi +import dicom_viewer.threeD.loaddicomfile as ldf +from dicom_viewer.threeD.matinqt import CFigureCanvas + + +class C3dView(QDialog): + + def __init__(self): + super().__init__() + loadUi('vol_view_module.ui', self) + self.setWindowTitle('3D View') + self.image = None + self.setMouseTracking(False) + self.imgs = None + self.patient = None + self.threshold = 1000 + self.step = 10 + self.alpha = 0.5 + self.thresholdEdit.setText(str(self.threshold)) + self.stepEdit.setText(str(self.step)) + self.alphaEdit.setText(str(self.alpha)) + self.refreshButton.clicked.connect(self.refresh_clicked) + self.graphicsView.setMinimumSize(1, 1) + + def refresh_clicked(self): + self.threshold = int(self.thresholdEdit.text()) + self.step = int(self.stepEdit.text()) + self.alpha = float(self.alphaEdit.text()) + self.vol_show() + + def vol_show(self): + print("Shape before resampling\t", self.imgs.shape) + imgs_after_resamp, spacing = ldf.resample(self.imgs, self.patient) + print("Shape after resampling\t", imgs_after_resamp.shape) + try: + v, f = ldf.make_mesh(imgs_after_resamp, threshold=self.threshold, step_size=self.step) + except Exception: + print('re-resample...') + imgs_after_resamp, spacing = ldf.resample(self.imgs, self.patient) + v, f = ldf.make_mesh(imgs_after_resamp, threshold=self.threshold, step_size=self.step) + + dr = CFigureCanvas() + dr.plt_3d(v, f, alpha=self.alpha) + graphicscene = QGraphicsScene() + graphicscene.addWidget(dr) + self.graphicsView.setScene(graphicscene) + self.graphicsView.show() + + +if __name__ == '__main__': + app = QApplication(sys.argv) + ex = C3dView() + ex.show() + sys.exit(app.exec_()) diff --git a/dicom_viewer/dicom_viewer/threeD/vol_view_module.ui b/dicom_viewer/dicom_viewer/threeD/vol_view_module.ui new file mode 100644 index 0000000..42c322e --- /dev/null +++ b/dicom_viewer/dicom_viewer/threeD/vol_view_module.ui @@ -0,0 +1,159 @@ + + + Dialog + + + Qt::NonModal + + + + 0 + 0 + 796 + 763 + + + + Dialog + + + + + + + + + + + + + 0 + 0 + + + + Step Size + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + Threshold + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + + + + + 0 + 0 + + + + + + + + + 0 + 0 + + + + + + + + + 0 + 0 + + + + Alpha + + + Qt::AlignCenter + + + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Refresh + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + + + + + + + + diff --git a/dicom_viewer/dicom_viewer/twoD/edgefunction.py b/dicom_viewer/dicom_viewer/twoD/edgefunction.py new file mode 100644 index 0000000..9c0d3a2 --- /dev/null +++ b/dicom_viewer/dicom_viewer/twoD/edgefunction.py @@ -0,0 +1,149 @@ +# flake8: noqa: 501 + +import cv2 +import numpy as np + + +def roberts(img): + ker_r1 = np.array([[-1, 0], [0, 1]]) + ker_r2 = np.array([[0, -1], [1, 0]]) + rows, cols = img.shape + # for center在左上角 + temp_img = cv2.copyMakeBorder(src=img, top=0, bottom=1, left=0, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+2, j:j+2] + new_img[i, j] = np.sqrt(np.sum(np.multiply(ker_r1, temp))**2 + np.sum(np.multiply(ker_r2, temp))**2) + return new_img + + +def perwitt(img): + ker_p1 = np.array([[-1, -1, -1], [0, 0, 0], [1, 1, 1]]) + ker_p2 = np.array([[-1, 0, 1], [-1, 0, 1], [-1, 0, 1]]) + rows, cols = img.shape + # for center在左上角 + temp_img = cv2.copyMakeBorder(src=img, top=1, bottom=1, left=1, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+3, j:j+3] + # 平方相加開根號 + new_img[i, j] = np.sqrt(np.sum(np.multiply(ker_p1, temp)) ** 2 + np.sum(np.multiply(ker_p2, temp)) ** 2) + return new_img + + +def sobel(img): + ker_p1 = np.array([[-1, -2, -1], [0, 0, 0], [1, 2, 1]]) + ker_p2 = np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]]) + rows, cols = img.shape + temp_img = cv2.copyMakeBorder(src=img, top=1, bottom=1, left=1, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+3, j:j+3] + # 平方相加開根號 + new_img[i, j] = np.sqrt(np.sum(np.multiply(ker_p1, temp))**2 + np.sum(np.multiply(ker_p2, temp))**2) + return new_img + + +def frei_chen(img): + ker_p1 = np.array([[-1, -np.sqrt(2), -1], [0, 0, 0], [1, np.sqrt(2), 1]]) + ker_p2 = np.array([[-1, 0, 1], [-np.sqrt(2), 0, np.sqrt(2)], [-1, 0, 1]]) + rows, cols = img.shape + temp_img = cv2.copyMakeBorder(src=img, top=1, bottom=1, left=1, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+3, j:j+3] + # 平方相加開根號 + new_img[i, j] = np.sqrt(np.sum(np.multiply(ker_p1, temp))**2 + np.sum(np.multiply(ker_p2, temp))**2) + return new_img + + +def krisch(img): + ker_k0 = np.array([[-3, -3, 5], [-3, 0, 5], [-3, -3, 5]]) + ker_k1 = np.array([[-3, 5, 5], [-3, 0, 5], [-3, -3, -3]]) + ker_k2 = np.array([[5, 5, 5], [-3, 0, -3], [-3, -3, -3]]) + ker_k3 = np.array([[5, 5, -3], [5, 0, -3], [-3, -3, -3]]) + ker_k4 = np.array([[5, -3, -3], [5, 0, -3], [5, -3, -3]]) + ker_k5 = np.array([[-3, -3, -3], [5, 0, -3], [5, 5, -3]]) + ker_k6 = np.array([[-3, -3, -3], [-3, 0, -3], [5, 5, 5]]) + ker_k7 = np.array([[-3, -3, -3], [-3, 0, 5], [-3, 5, 5]]) + list_kn = [ker_k0, ker_k1, ker_k2, ker_k3, ker_k4, ker_k5, ker_k6, ker_k7] + rows, cols = img.shape + temp_img = cv2.copyMakeBorder(src=img, top=1, bottom=1, left=1, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+3, j:j+3] + _max = 0 # 初始化max值 + for ker in list_kn: + temp_sum = np.sum(ker * temp) + if temp_sum > _max: + _max = temp_sum + new_img[i, j] = _max + return new_img + + +def robinson(img): + ker_r0 = np.array([[-1, 0, 1], [-2, 0, 2], [-1, 0, 1]]) + ker_r1 = np.array([[0, 1, 2], [-1, 0, 1], [-2, -1, 0]]) + ker_r2 = np.array([[1, 2, 1], [0, 0, 0], [-1, -2, -1]]) + ker_r3 = np.array([[2, 1, 0], [1, 0, -1], [0, -1, -2]]) + ker_r4 = np.array([[1, 0, -1], [2, 0, -2], [1, 0, -1]]) + ker_r5 = np.array([[0, -1, -2], [1, 0, -1], [2, 1, 0]]) + ker_r6 = np.array([[-1, -2, -1], [0, 0, 0], [1, 2, 1]]) + ker_r7 = np.array([[-2, -1, 0], [-1, 0, 1], [0, 1, 2]]) + list_rn = [ker_r0, ker_r1, ker_r2, ker_r3, ker_r4, ker_r5, ker_r6, ker_r7] + rows, cols = img.shape + temp_img = cv2.copyMakeBorder(src=img, top=1, bottom=1, left=1, right=1, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+3, j:j+3] + _max = 0 # 初始化max值 + for ker in list_rn: + temp_sum = np.sum(ker * temp) + if temp_sum > _max: + _max = temp_sum + new_img[i, j] = _max + return new_img + + +def nevatia_babu(img): + ker_nb0 = np.array([[100, 100, 100, 100, 100], [100, 100, 100, 100, 100], [0, 0, 0, 0, 0], + [-100, -100, -100, -100, -100], [-100, -100, -100, -100, -100]]) + ker_nb1 = np.array([[100, 100, 100, 100, 100], [100, 100, 100, 78, -32], [100, 92, 0, -92, -100], + [32, -78, -100, -100, -100], [-100, -100, -100, -100, -100]]) + ker_nb2 = np.array([[100, 100, 100, 32, -100], [100, 100, 92, -78, -100], [100, 100, 0, -100, -100], + [100, 78, -92, -100, -100], [100, -32, -100, -100, -100]]) + ker_nb3 = np.array([[-100, -100, 0, 100, 100], [-100, -100, 0, 100, 100], [-100, -100, 0, 100, 100], + [-100, -100, 0, 100, 100], [-100, -100, 0, 100, 100]]) + ker_nb4 = np.array([[-100, 32, 100, 100, 100], [-100, -78, 92, 100, 100], [-100, -100, 0, 100, 100], + [-100, -100, -92, 78, 100], [-100, -100, -100, -32, 100]]) + ker_nb5 = np.array([[100, 100, 100, 100, 100], [-32, 78, 100, 100, 100], [-100, -92, 0, 92, 100], + [-100, -100, -100, -78, 32], [-100, -100, -100, -100, -100]]) + + list_nbn = [ker_nb0, ker_nb1, ker_nb2, ker_nb3, ker_nb4, ker_nb5] + rows, cols = img.shape + temp_img = cv2.copyMakeBorder(src=img, top=2, bottom=2, left=2, right=2, borderType=cv2.BORDER_REPLICATE) + new_img = img.copy().astype(float) + for i in range(rows): + for j in range(cols): + temp = temp_img[i:i+5, j:j+5] + _max = 0 # 初始化max值 + for ker in list_nbn: + temp_sum = np.sum(ker * temp) + if temp_sum > _max: + _max = temp_sum + new_img[i, j] = _max + return new_img + + +def reverse_thresholding(img, threshold=128): + new_img = np.empty(img.shape) + new_img.fill(255) + mask = img >= threshold + new_img[mask] = 0 + return new_img diff --git a/dicom_viewer/dicom_viewer/twoD/qpaintlabel2.py b/dicom_viewer/dicom_viewer/twoD/qpaintlabel2.py new file mode 100644 index 0000000..0e287d3 --- /dev/null +++ b/dicom_viewer/dicom_viewer/twoD/qpaintlabel2.py @@ -0,0 +1,224 @@ +# flake8: noqa: 501 + +from PyQt5.QtWidgets import QLabel +from PyQt5.QtGui import QImage, QPixmap +from PyQt5 import QtCore +from PyQt5.QtCore import Qt +from twoD import edgefunction as ef +import cv2 +import numpy as np +import pydicom + + +class QPaintLabel2(QLabel): + + def __init__(self, parent): + super(QLabel, self).__init__(parent) + self.window = None + self.setMouseTracking(False) + # 為了能resize scale down, 要setMinimumSize，不然會被辨認為不可縮小 + self.setMinimumSize(1, 1) + self.drawornot, self.seed = False, False + self.image = None + self.processedImage = None + self.imgr, self.imgc = None, None + self.pos_x = 20 + self.pos_y = 20 + self.imgpos_x = 0 + self.imgpos_y = 0 + # 遇到list就停，圖上的顯示白色只是幌子 + self.pos_xy = [] + self.mor_Kersize = 3 + self.mor_Iter = 3 + + def mouseMoveEvent(self, event): + if self.drawornot: + self.pos_x = event.pos().x() + self.pos_y = event.pos().y() + self.imgpos_x = int(self.pos_x * self.imgc / self.width()) + self.imgpos_y = int(self.pos_y * self.imgr / self.height()) + self.pos_xy.append((self.imgpos_x, self.imgpos_y)) + self.drawing() + + def mousePressEvent(self, event): + if self.drawornot: + self.pos_x = event.pos().x() + self.pos_y = event.pos().y() + self.imgpos_x = int(self.pos_x * self.imgc / self.width()) + self.imgpos_y = int(self.pos_y * self.imgr / self.height()) + self.pos_xy.append((self.imgpos_x, self.imgpos_y)) + self.drawing() + if self.seed: + self.pos_x = event.pos().x() + self.pos_y = event.pos().y() + self.imgpos_x = int(self.pos_x * self.imgc / self.width()) + self.imgpos_y = int(self.pos_y * self.imgr / self.height()) + self.seed_clicked(seedx=self.imgpos_x, seedy=self.imgpos_y) + self.seed = False + +# https://stackoverflow.com/questions/7501706/python-how-do-i-pass-variables-between-class-instances-or-get-the-caller + def edge_detection(self, _type): + try: + self.processedImage = cv2.cvtColor(self.processedImage, cv2.COLOR_BGR2GRAY) + except Exception: + pass + self.processedImage = linear_convert(self.processedImage).astype(np.uint8) + if _type == 'Laplacian': + self.processedImage = cv2.convertScaleAbs(cv2.Laplacian(self.processedImage, cv2.CV_16S, ksize=1)) + elif _type == 'Sobel': + img = linear_convert(ef.sobel(self.processedImage)).astype(np.uint8) + ret, img = cv2.threshold(img, 110, 255, cv2.THRESH_BINARY) + self.processedImage = img + elif _type == 'Perwitt': + img = linear_convert(ef.perwitt(self.processedImage)).astype(np.uint8) + ret, img = cv2.threshold(img, 70, 255, cv2.THRESH_BINARY) + self.processedImage = img + elif _type == 'Frei & Chen': + img = linear_convert(ef.frei_chen(self.processedImage)).astype(np.uint8) + ret, img = cv2.threshold(img, 80, 255, cv2.THRESH_BINARY) + self.processedImage = img + + self.display_image() + + def morthology(self, _type): + kernel = np.ones((self.mor_Kersize, self.mor_Kersize), np.uint8) + + if _type == 'Dilation': + self.processedImage = cv2.dilate(self.processedImage, kernel, iterations=self.mor_Iter) + elif _type == 'Erosion': + self.processedImage = cv2.erode(self.processedImage, kernel, iterations=self.mor_Iter) + elif _type == 'Opening': + self.processedImage = cv2.morphologyEx(self.processedImage, cv2.MORPH_OPEN, + kernel, iterations=self.mor_Iter) + elif _type == 'Closing': + self.processedImage = cv2.morphologyEx(self.processedImage, cv2.MORPH_CLOSE, + kernel, iterations=self.mor_Iter) + + self.display_image() + + def load_dicom_image(self, fname): + dcm = pydicom.read_file(fname, force=True) + # or whatever is the correct transfer syntax for the file + dcm.file_meta.TransferSyntaxUID = pydicom.uid.ImplicitVRLittleEndian + print(np.nanmax(dcm.pixel_array), np.nanmin(dcm.pixel_array)) + dcm.image = dcm.pixel_array * dcm.RescaleSlope + dcm.RescaleIntercept + self.image = linear_convert(dcm.image).astype(np.uint8) + self.processedImage = self.image.copy() + self.imgr, self.imgc = self.processedImage.shape[0:2] + self.display_image() + + def load_image(self, fname): + print(fname) + self.image = cv2.imread(fname) + self.processedImage = self.image.copy() + self.imgr, self.imgc = self.processedImage.shape[0:2] + self.display_image() + + def display_image(self): + qformat = QImage.Format_Indexed8 + if len(self.processedImage.shape) == 3: # rows[0], cols[1], channels[2] + if (self.processedImage.shape[2]) == 4: + qformat = QImage.Format_RGBA8888 + else: + qformat = QImage.Format_RGB888 + + w, h = self.width(), self.height() + img = QImage(self.processedImage, self.processedImage.shape[1], + self.processedImage.shape[0], self.processedImage.strides[0], qformat) + img = img.rgbSwapped() + # 讓image跟著Qlabel大小改變 + self.setScaledContents(True) + # 扣掉線寬才不會讓qlabel變大 + backlash = self.lineWidth()*2 + self.setPixmap(QPixmap.fromImage(img).scaled(w-backlash, h-backlash, Qt.IgnoreAspectRatio)) + self.setAlignment(QtCore.Qt.AlignHCenter | QtCore.Qt.AlignVCenter) + + def drawing(self): + self.processedImage[self.imgpos_y:self.imgpos_y+20, self.imgpos_x:self.imgpos_x+20] = 255 + self.display_image() + + def thresholding(self, threshold): + ret, img = cv2.threshold(self.processedImage, threshold, 255, cv2.THRESH_BINARY) + self.processedImage = img + self.display_image() + + def seed_clicked(self, seedx, seedy): + try: + self.processedImage = cv2.cvtColor(self.processedImage, cv2.COLOR_BGR2GRAY) + except Exception: + pass + tobeprocessed = self.processedImage.copy() + result = self.region_growing(tobeprocessed, (seedy, seedx)) + self.processedImage = result + self.display_image() + + def region_growing(self, img, seed): + _list = [] + # zeros_like建構一個和img的shape一樣但都是zeros的矩陣，和np.zeros(img.shape)應該是一樣的 + outimg = np.zeros_like(img) + _list.append((seed[0], seed[1])) + processed = [] + # 當list有值，就挑第一個list[0]進行處理，如果list[0]的值不是0(即是255)，就把新影像list[0]位置設為255 + # 周遭是255的位置在新影像也都設成255，並進入待處理區 + while len(_list) > 0: + pix = _list[0] + outimg[pix[0], pix[1]] = 255 + for coord in get8n(pix[0], pix[1], img.shape): + if img[coord[0], coord[1]] != 0: + outimg[coord[0], coord[1]] = 255 + # 放在list裡面的都會被拿出來處理，所以如果不在processed裡面，代表還沒被放到預備處理區（即list） + # 就把他放進去list，然後也放進processed，代表已經進入預備處理區了 + if coord not in processed: + _list.append(coord) + processed.append(coord) + # 處理過的就離開預備處理清單list, processed不刪掉，因為做過的就是做過了 + _list.pop(0) + self.processedImage = outimg + self.display_image() + cv2.destroyAllWindows() + return outimg + + +def get8n(x, y, shape): + out = [] + maxx = shape[1]-1 + maxy = shape[0]-1 + # top left + outx = min(max(x-1, 0), maxx) + outy = min(max(y-1, 0), maxy) + out.append((outx, outy)) + # top center + outx = x + outy = min(max(y-1, 0), maxy) + out.append((outx, outy)) + # top right + outx = min(max(x+1, 0), maxx) + outy = min(max(y-1, 0), maxy) + out.append((outx, outy)) + # left + outx = min(max(x-1, 0), maxx) + outy = y + out.append((outx, outy)) + # right + outx = min(max(x+1, 0), maxx) + outy = y + out.append((outx, outy)) + # bottom left + outx = min(max(x-1, 0), maxx) + outy = min(max(y+1, 0), maxy) + out.append((outx, outy)) + # bottom center + outx = x + outy = min(max(y+1, 0), maxy) + out.append((outx, outy)) + # bottom right + outx = min(max(x+1, 0), maxx) + outy = min(max(y+1, 0), maxy) + out.append((outx, outy)) + return out + + +def linear_convert(img): + convert_scale = 255.0 / (np.max(img) - np.min(img)) + converted_img = convert_scale*img-(convert_scale*np.min(img)) + return converted_img diff --git a/dicom_viewer/dicom_viewer/twoD/resources/undo.png b/dicom_viewer/dicom_viewer/twoD/resources/undo.png new file mode 100644 index 0000000..904b6f6 Binary files /dev/null and b/dicom_viewer/dicom_viewer/twoD/resources/undo.png differ diff --git a/dicom_viewer/dicom_viewer/twoD/twoD_module.py b/dicom_viewer/dicom_viewer/twoD/twoD_module.py new file mode 100644 index 0000000..00b3a0e --- /dev/null +++ b/dicom_viewer/dicom_viewer/twoD/twoD_module.py @@ -0,0 +1,111 @@ +# flake8: noqa: 501 + +import sys +from PyQt5.QtWidgets import QDialog, QFileDialog, QApplication, QMessageBox +from PyQt5.QtGui import QIcon +from PyQt5.uic import loadUi +import os +import cv2 + + +class CtwoD(QDialog): + def __init__(self): + super().__init__() + path = os.getcwd() + os.chdir(path+'/twoD') + loadUi('twoD_module.ui', self) + self.setWindowTitle('2D Processing') + self.image = None + self.setMouseTracking(False) + self.loadButton.clicked.connect(self.load_clicked) + self.saveButton.clicked.connect(self.save_clicked) + self.drawButton.setCheckable(True) + self.drawButton.clicked.connect(self.draw_clicked) + self.seedButton.clicked.connect(self.seed_clicked) + self.thresholdButton.clicked.connect(self.threshold_clicked) + self.imgLabel_1.window = 1 + self.imgLabel_2.window = 2 + self.morButton.clicked.connect(self.mor_clicked) + self.kersizeEdit.setText(str(self.imgLabel_2.mor_Kersize)) + self.iterEdit.setText(str(self.imgLabel_2.mor_Iter)) + self.edgeButton.clicked.connect(self.edge_clicked) + self.undoButton.clicked.connect(self.undo_clicked) + self.undoButton.setIcon(QIcon('resources/undo.png')) + self.undoButton.setText('') + self.grayButton.clicked.connect(self.gray_clicked) + + def resizeEvent(self, event): + super().resizeEvent(event) + if self.imgLabel_1.processedImage is not None: + self.imgLabel_1.display_image() + if self.imgLabel_2.processedImage is not None: + self.imgLabel_2.display_image() + + def gray_clicked(self): + try: + self.imgLabel_1.processedImage = cv2.cvtColor(self.imgLabel_1.processedImage, cv2.COLOR_BGR2GRAY) + self.imgLabel_2.processedImage = cv2.cvtColor(self.imgLabel_2.processedImage, cv2.COLOR_BGR2GRAY) + except Exception: + QMessageBox.about(None, 'Information', 'Already converted to grayscaled!') + self.imgLabel_1.display_image() + self.imgLabel_2.display_image() + + def undo_clicked(self): + self.imgLabel_1.processedImage = self.imgLabel_1.image.copy() + self.imgLabel_1.display_image() + self.imgLabel_2.processedImage = self.imgLabel_2.image.copy() + self.imgLabel_2.display_image() + + def edge_clicked(self): + self.imgLabel_2.edge_detection(self.edgeBox.itemText(self.edgeBox.currentIndex())) + + def mor_clicked(self): + self.imgLabel_2.mor_Kersize = int(self.kersizeEdit.text()) + self.imgLabel_2.mor_Iter = int(self.iterEdit.text()) + self.imgLabel_2.morthology(self.morBox.itemText(self.morBox.currentIndex())) + + def threshold_clicked(self): + threshold = int(self.thresholdValue.text()) + self.imgLabel_2.thresholding(threshold) + + def seed_clicked(self): + self.imgLabel_2.seed = True + + def draw_clicked(self, status): + if status: + self.imgLabel_2.drawornot = True + self.drawButton.setText('Start drawimg') + else: + self.imgLabel_2.drawornot = False + self.drawButton.setText('Stop drawimg') + + def save_clicked(self): + fname, _filter = QFileDialog.getSaveFileName(self, 'save file', '~/untitled', "Image Files (*.jpg)") + if fname: + cv2.imwrite(fname, self.imgLabel_2.processedImage) + else: + print('Error') + + def load_clicked(self): + fname, _filter = QFileDialog.getOpenFileName(self, 'open file', '~/Desktop', + "Image Files (*.jpg *.png *.bmp *.dcm *DCM)") + name, extension = os.path.splitext(fname) + print(extension) + if extension.lower() == '.dcm': + self.imgLabel_1.load_dicom_image(fname) + self.imgLabel_2.load_dicom_image(fname) + else: + try: + self.imgLabel_1.load_image(fname) + self.imgLabel_2.load_image(fname) + except Exception: + print('Error') + finally: + print('Finish loading') + + +if __name__ == '__main__': + app = QApplication(sys.argv) + ex = CtwoD() + ex.show() + sys.exit(app.exec_()) diff --git a/dicom_viewer/dicom_viewer/twoD/twoD_module.ui b/dicom_viewer/dicom_viewer/twoD/twoD_module.ui new file mode 100644 index 0000000..6863fce --- /dev/null +++ b/dicom_viewer/dicom_viewer/twoD/twoD_module.ui @@ -0,0 +1,443 @@ + + + Dialog + + + Qt::NonModal + + + + 0 + 0 + 942 + 670 + + + + Dialog + + + + + + + + + + Gray +Scale + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + + + + Undo + + + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + 0 + + + + true + + + Threshold + + + + + + + + + + + Qt::NoFocus + + + Thresholding + + + + + + + + + + Morthology + + + + + + + + + Dilation + + + + + Erosion + + + + + Opening + + + + + Closing + + + + + + + + + + + + + 0 + 0 + + + + Iteration + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + + + + + 0 + 0 + + + + Kernel Size + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Refresh + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 40 + 20 + + + + + + + + + + + + + Edge Detection + + + + + + + + + 0 + 0 + + + + + Laplacian + + + + + Sobel + + + + + Perwitt + + + + + Frei & Chen + + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Go! + + + + + + + + + + Region Growing + + + + + + + + Currently Only Support Binary Image + + + Qt::AlignCenter + + + + + + + + 0 + 0 + + + + Qt::NoFocus + + + Seeding + + + + + + + + + + Drawing + + + + + + + + true + + + Qt::NoFocus + + + -1 + + + Draw + + + + + + + + + + + + + + + + + Qt::NoFocus + + + Load Image + + + + + + + Qt::NoFocus + + + Save Image + + + + + + + + + + + + 0 + 0 + + + + QFrame::Box + + + 1 + + + + + + + + + + + 0 + 0 + + + + QFrame::Box + + + QFrame::Plain + + + 1 + + + + + + + + + + + + + QPaintLabel2 + QLabel +
qpaintlabel2.h
+ + + + + + thresholdValue + returnPressed() + thresholdButton + click() + + + 377 + 86 + + + 296 + 89 + + + + + diff --git a/dicom_viewer/package.xml b/dicom_viewer/package.xml new file mode 100644 index 0000000..f24dc89 --- /dev/null +++ b/dicom_viewer/package.xml @@ -0,0 +1,21 @@ + + + + dicom_viewer + 0.0.0 + Dicom viewer + telecom + TODO: License declaration + + rclpy + std_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/dicom_viewer/readme.md b/dicom_viewer/readme.md new file mode 100644 index 0000000..908f867 --- /dev/null +++ b/dicom_viewer/readme.md @@ -0,0 +1,82 @@ +

+ + Dicom Viewer + +

簡易醫學影像GUI (Dicom Viewer)

+

能顯示 2D/3D Dicom影像的應用

+
+

+ +本project旨在利用python+Qt製作簡易的醫學影像GUI，提供一個平台，能在上面使用python開發測試各式影像處理功能，尤其是針對3D之Dicom Stack！ + +## 先看兩段Demo吧 +### 2D Image Processing +### 3D Image Processing + +## 執行畫面 +執行程式會打開Main Window，左上角的選單有2D processing和3D processing兩個子選項，其中後者embed有3D volume reconstruction功能 + +### Main Window + + mainwindow + + +### 2D processing +內含功能 +- Load Image (含*.dcm) +- Save Image +- Convert to gray scale +- Restore +- Thresholding +- Region Growing +- Morthology (Dilation, Erosion, Opening, Closing) +- Edge Detection (Laplacian, Sobel, Perwitt, Frei & Chen) +- Drawing +
+ + 2D_Processing + + + +### 3D processing +內含功能 +- Load DICOM stack +- Save slice (axial, sagittal, coronal) +- Colormap transform +- Slider scrolling +- Mouse hovering/clicking +- Show DICOM info +- Show slice index coordinate +- 3D volume reconstruction +
+ + 3D_Processing + + +### 3D volume reconstruction +
+ + 3D_Volume + + + +## Future Extension +- 加入數據分析功能 + >導入一些醫學知識，讓軟體有一些初步的分析功能（如有沒有骨折、腫瘤等等） +- 3D reconstruction改善 + >更新volume重建算法，提升計算速度與還原度 +- 3D image processing + >加入對3D voxel影像進行更複雜的影像處理功能 +- Make it distributable + >包裝成release版本，成為可轉散發軟體。希望能支援跨平台運作（window、macOS） + +## How to use it? +Project root will be **/src**, just clone it and run mainwindow.py. + +Strictly follow the package version in requirements.txt is not necessary. + +## Acknowledgments +- [brain.png](https://github.com/wenyalintw/Dicom-Viewer/blob/master/resources/brain.png) licensed under "CC BY 3.0" downloaded from [ICONFINDER](https://www.iconfinder.com/icons/1609653/brain_organs_icon) +- 3D volume reconstruction modified from [Howard Chen's Post](https://www.raddq.com/dicom-processing-segmentation-visualization-in-python/) + +###### MIT License (2019), Wen-Ya Lin diff --git a/dicom_viewer/resource/dicom_viewer b/dicom_viewer/resource/dicom_viewer new file mode 100644 index 0000000..e69de29 diff --git a/dicom_viewer/setup.cfg b/dicom_viewer/setup.cfg new file mode 100644 index 0000000..291bd84 --- /dev/null +++ b/dicom_viewer/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/dicom_viewer +[install] +install_scripts=$base/lib/dicom_viewer diff --git a/dicom_viewer/setup.py b/dicom_viewer/setup.py new file mode 100644 index 0000000..337aa3b --- /dev/null +++ b/dicom_viewer/setup.py @@ -0,0 +1,27 @@ +from setuptools import find_packages, setup + +package_name = 'dicom_viewer' + +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='telecom', + maintainer_email='lfmaldon@ens2m.org', + description='Dicom viewer', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + # 'pointPublisher = dicom_viewer.pointPublisher:main', + 'mainwindow = dicom_viewer.mainwindow:main' + ], + }, +) diff --git a/dicom_viewer/test/test_copyright.py b/dicom_viewer/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/dicom_viewer/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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' diff --git a/dicom_viewer/test/test_flake8.py b/dicom_viewer/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/dicom_viewer/test/test_flake8.py @@ -0,0 +1,25 @@ +# 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) diff --git a/dicom_viewer/test/test_pep257.py b/dicom_viewer/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/dicom_viewer/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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' diff --git a/doc/placeholder.txt b/doc/placeholder.txt new file mode 100644 index 0000000..aaf02a3 --- /dev/null +++ b/doc/placeholder.txt @@ -0,0 +1,3 @@ +place holder for report + +Has no value, dreams or aspirations, only for testing \ No newline at end of file diff --git a/haptic_controller/CMakeLists.txt b/haptic_controller/CMakeLists.txt new file mode 100644 index 0000000..38132dc --- /dev/null +++ b/haptic_controller/CMakeLists.txt @@ -0,0 +1,123 @@ +cmake_minimum_required(VERSION 3.8) +project(haptic_controller) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies + +set(DEPS + ament_cmake + ament_cmake_python + hardware_interface + rclcpp + rclpy + trajectory_msgs + tf2_ros + tf2_geometry_msgs + visualization_msgs + geometry_msgs + std_msgs + interactive_markers + controller_interface + generate_parameter_library + pluginlib + rclcpp_lifecycle + realtime_tools + pantograph_library + eigen3_cmake_module + Eigen3 +) + +foreach(Dependency IN ITEMS ${DEPS}) + find_package(${Dependency} REQUIRED) +endforeach() + +generate_parameter_library( + haptic_controller_parameters + src/haptic_controller_parameters.yaml +) +add_library(${PROJECT_NAME} SHARED + src/haptic_controller.cpp +) + +target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ + $ +) + +target_link_libraries(${PROJECT_NAME} PUBLIC + haptic_controller_parameters +) +ament_target_dependencies(${PROJECT_NAME} PUBLIC ${DEPS}) +target_compile_definitions(${PROJECT_NAME} PRIVATE "HAPTIC_CONTROLLER_BUILDING_DLL") +pluginlib_export_plugin_description_file(controller_interface haptic_controller_plugin.xml) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +#----------------------------------------------------- +# Cpp setup +#----------------------------------------------------- + +# Include Cpp "include" directory +# include_directories(include) + +# Create Cpp executables +add_executable(error_estimation src/error_estimation.cpp) +ament_target_dependencies(error_estimation PUBLIC ${DEPS}) + +# Install Cpp executables +install( + TARGETS + error_estimation + DESTINATION lib/${PROJECT_NAME}) + +install( + DIRECTORY include/ + DESTINATION include +) +install( + TARGETS + haptic_controller + haptic_controller_parameters + EXPORT export_haptic_controller + RUNTIME DESTINATION bin + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + INCLUDES DESTINATION include +) + +#----------------------------------------------------- +# Install Python package +#----------------------------------------------------- +ament_python_install_package( + ${PROJECT_NAME} +) + +install( + PROGRAMS + scripts/markers.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_export_targets(export_haptic_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${DEPS}) +ament_package() diff --git a/haptic_controller/LICENSE b/haptic_controller/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/haptic_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/haptic_controller/haptic_controller/__init__.py b/haptic_controller/haptic_controller/__init__.py new file mode 100644 index 0000000..8824178 --- /dev/null +++ b/haptic_controller/haptic_controller/__init__.py @@ -0,0 +1,15 @@ +# Copyright 2023, ICube Laboratory, University of Strasbourg +# +# 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. + +# Author: Luis Maldonado (lfmaldon@ens2m.org) diff --git a/haptic_controller/haptic_controller_plugin.xml b/haptic_controller/haptic_controller_plugin.xml new file mode 100644 index 0000000..04b5129 --- /dev/null +++ b/haptic_controller/haptic_controller_plugin.xml @@ -0,0 +1,10 @@ + + + + Haptic controller used to implement force control + + + diff --git a/haptic_controller/include/haptic_controller/haptic_controller.hpp b/haptic_controller/include/haptic_controller/haptic_controller.hpp new file mode 100644 index 0000000..4149706 --- /dev/null +++ b/haptic_controller/include/haptic_controller/haptic_controller.hpp @@ -0,0 +1,109 @@ +// Copyright 2024, ICube Laboratory, University of Strasbourg +// +// 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. + +#ifndef HAPTIC_CONTROLLER__HAPTIC_CONTROLLER_HPP_ +#define HAPTIC_CONTROLLER__HAPTIC_CONTROLLER_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "realtime_tools/realtime_buffer.h" +#include "haptic_controller/visibility_control.h" + +// auto-generated by generate_parameter_library +#include "haptic_controller_parameters.hpp" +#include "pantograph_library/pantograph_model.hpp" + +#include "std_msgs/msg/float64.hpp" +#include + +namespace haptic_controller +{ + +class HapticController : public controller_interface::ControllerInterface +{ +public: + HAPTIC_CONTROLLER_PUBLIC + HapticController(); + + HAPTIC_CONTROLLER_PUBLIC + ~HapticController() = default; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_init() override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + HAPTIC_CONTROLLER_PUBLIC + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + std::vector joint_names_; + std::string interface_name_; + + std::vector command_interface_types_; + Eigen::Vector3d marker_pos_; + double last_pos_a1_; + double last_pos_a5_; + double current_pos_a1_; + double current_pos_a5_; + +protected: + controller_interface::CallbackReturn read_parameters(); + void marker_callback(const visualization_msgs::msg::Marker::SharedPtr msg); + + std::shared_ptr param_listener_; + Params params_; + + std::unordered_map> name_if_value_mapping_; + std::vector pantograph_joint_names_; + pantograph_library::PantographModel pantograph_model_; + + realtime_tools::RealtimeBuffer> rt_command_ptr_; + rclcpp::Subscription::SharedPtr error_subscriber_; + double last_error_; + rclcpp::Subscription::SharedPtr marker_subscriber_; + double kp; + double kd_tip; + double ratio; +}; + +} // namespace haptic_controller + +#endif // HAPTIC_CONTROLLER__HAPTIC_CONTROLLER_HPP_ diff --git a/haptic_controller/include/haptic_controller/visibility_control.h b/haptic_controller/include/haptic_controller/visibility_control.h new file mode 100644 index 0000000..27fa911 --- /dev/null +++ b/haptic_controller/include/haptic_controller/visibility_control.h @@ -0,0 +1,56 @@ +// Copyright 2024, ICube Laboratory, University of Strasbourg +// +// 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. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef HAPTIC_CONTROLLER__VISIBILITY_CONTROL_H_ +#define HAPTIC_CONTROLLER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define HAPTIC_CONTROLLER_EXPORT __attribute__((dllexport)) +#define HAPTIC_CONTROLLER_IMPORT __attribute__((dllimport)) +#else +#define HAPTIC_CONTROLLER_EXPORT __declspec(dllexport) +#define HAPTIC_CONTROLLER_IMPORT __declspec(dllimport) +#endif +#ifdef HAPTIC_CONTROLLER_BUILDING_DLL +#define HAPTIC_CONTROLLER_PUBLIC HAPTIC_CONTROLLER_EXPORT +#else +#define HAPTIC_CONTROLLER_PUBLIC HAPTIC_CONTROLLER_IMPORT +#endif +#define HAPTIC_CONTROLLER_PUBLIC_TYPE HAPTIC_CONTROLLER_PUBLIC +#define HAPTIC_CONTROLLER_LOCAL +#else +#define HAPTIC_CONTROLLER_EXPORT __attribute__((visibility("default"))) +#define HAPTIC_CONTROLLER_IMPORT +#if __GNUC__ >= 4 +#define HAPTIC_CONTROLLER_PUBLIC __attribute__((visibility("default"))) +#define HAPTIC_CONTROLLER_LOCAL __attribute__((visibility("hidden"))) +#else +#define HAPTIC_CONTROLLER_PUBLIC +#define HAPTIC_CONTROLLER_LOCAL +#endif +#define HAPTIC_CONTROLLER_PUBLIC_TYPE +#endif + +#endif // HAPTIC_CONTROLLER__VISIBILITY_CONTROL_H_ diff --git a/haptic_controller/package.xml b/haptic_controller/package.xml new file mode 100644 index 0000000..74fba1e --- /dev/null +++ b/haptic_controller/package.xml @@ -0,0 +1,33 @@ + + + + haptic_controller + 0.0.0 + Haptic controller and visualization tools + telecom + Apache-2.0 + + ament_cmake + ament_cmake_python + + pantograph_library + rclcpp + rclcpp_lifecycle + rclpy + + backward_ros + controller_interface + generate_parameter_library + hardware_interface + pluginlib + + ament_lint_auto + ament_lint_common + + forward_command_controller + trajectory_msgs + + + ament_cmake + + diff --git a/haptic_controller/scripts/markers.py b/haptic_controller/scripts/markers.py new file mode 100755 index 0000000..9674e3e --- /dev/null +++ b/haptic_controller/scripts/markers.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from visualization_msgs.msg import Marker +from geometry_msgs.msg import Point +from std_msgs.msg import Float64MultiArray + + +class NeedleTrajectory(Node): + + def __init__(self): + super().__init__('needle_trajectory_marker') + + # Create subscription to topic /target_point + self.subscription = self.create_subscription(Float64MultiArray, '/image_points', + self.listener_callback, 5) + + self.subscription # Prevent unused variable warning + + self.publisher = self.create_publisher(Marker, '/visualization_marker', 5) + + def listener_callback(self, msg): + # Ensure the message contains 3 elements + if len(msg.data) != 6: + self.get_logger().error('Received Float64MultiArray does not contain exactly 6 elements.') # noqa: E501 + return + + # Extract the point coordinates + x1, y1, z1, x2, y2, z2 = msg.data + + # Initialize the marker + marker = Marker() + marker.header.frame_id = 'world' + marker.header.stamp = self.get_clock().now().to_msg() + marker.type = marker.LINE_STRIP + marker.id = 0 + marker.action = marker.ADD + + # Marker parameters + marker.scale.x = 0.005 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.pose.position.x = 0.0 + marker.pose.position.y = 0.0 + marker.pose.position.z = 0.0 + + # Marker line points + marker.points = [] + # Starting point (fulcrum point coords) + start_pt = Point() + start_pt.x = 0.0 + start_pt.y = 0.16056 + start_pt.z = 0.09 + marker.points.append(start_pt) + + # Create the target point + target_point = Point() + target_point.x = x2 + target_point.y = y2 + target_point.z = z2 + marker.points.append(target_point) + + self.publisher.publish(marker) + + +def main(args=None): + rclpy.init(args=args) + trajectory_node = NeedleTrajectory() + + try: + rclpy.spin(trajectory_node) + except KeyboardInterrupt: + pass + trajectory_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/haptic_controller/src/error_estimation.cpp b/haptic_controller/src/error_estimation.cpp new file mode 100644 index 0000000..ca6841c --- /dev/null +++ b/haptic_controller/src/error_estimation.cpp @@ -0,0 +1,157 @@ +// Copyright 2024, ICube Laboratory, University of Strasbourg +// +// 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. + +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include "visualization_msgs/msg/interactive_marker_feedback.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/float64.hpp" + +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + + +class ErrorEstimation : public rclcpp::Node +{ +public: + ErrorEstimation() + : Node("error_estimation"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) + { + marker_sub_ = this->create_subscription( + "/visualization_marker", 10, + std::bind(&ErrorEstimation::marker_callback, this, std::placeholders::_1)); + + // Create error publisher + error_publisher_ = this->create_publisher("/angle_error", 10); + + // Timer to trigger error calculation and logging every second (only for testing) + timer_ = + this->create_wall_timer( + std::chrono::milliseconds(2), + std::bind(&ErrorEstimation::calculate_and_log_error, this)); + + RCLCPP_INFO(this->get_logger(), "Error Estimation Node has been started."); + } + +private: + void marker_callback(const visualization_msgs::msg::Marker::SharedPtr msg) + { + if (msg->points.size() != 2) { + RCLCPP_ERROR(this->get_logger(), "Received marker does not have exactly 2 points."); + return; + } + + if (msg->points.size() == 0) { + // If msg is empty default points to origin + p0.x = PI_x; + p0.y = PI_y; + p0.z = PI_z; + + p1.x = 0; + p1.y = 0; + p1.z = 0; + } else { + // Extract the points from the marker + p0 = msg->points[0]; // Insertion point + p1 = msg->points[1]; // Target point from marker + } + } + + void calculate_and_log_error() + { + // calculate and log error + try { + auto transform_stamped = tf_buffer_.lookupTransform( + "world", "needle_interaction_link", + tf2::TimePointZero); + + // Needle insertion point + Eigen::Vector3d PI; + PI << PI_x, PI_y, PI_z; + + // Extract needle interaction point (PU) position + auto PU = transform_stamped.transform.translation; + Eigen::Vector3d PU_vec(PU.x, PU.y, PU.z); + + // Vector from insertion point (PI) to arrow tip + // Vectors are defined in PI frame + Eigen::Vector3d marker_vec(p1.x - p0.x, p1.y - p0.y, p1.z - p0.z); + + // Vector from PI to PU + Eigen::Vector3d needle_vec = PU_vec - PI; + + // Calculate the angle between the arrow vector + // and the vector from the fixed point to the third point + double angle_error = PI_CST - std::acos( + needle_vec.dot(marker_vec) / + (needle_vec.norm() * marker_vec.norm())); + + if (isnan(angle_error)) { + // RCLCPP_INFO( + // this->get_logger(), "Unable to calculate error, target point not defined ! "); + + // RCLCPP_INFO( + // this->get_logger(), "Angle error set to %.2f ", 0.0); + + // Publish angle error default value + auto msg = std_msgs::msg::Float64(); + msg.data = 0.0; + error_publisher_->publish(msg); + } else { + // RCLCPP_INFO( + // this->get_logger(), "Angular error between needle and trajectory: %.2f", angle_error); + + // Publish angle error + auto msg = std_msgs::msg::Float64(); + msg.data = angle_error; + error_publisher_->publish(msg); + } + } catch (tf2::TransformException & ex) { + // RCLCPP_ERROR( + // this->get_logger(), "Could not transform from 'world' to 'needle_interaction_link': %s", + // ex.what()); + } + } + + geometry_msgs::msg::Point p0; + geometry_msgs::msg::Point p1; + + // Coords of insertion point according to CAD + double PI_x = 0; + double PI_y = 0.16056; + double PI_z = 0.09; + + double PI_CST = 3.14159265358979323846; + + rclcpp::Subscription::SharedPtr marker_sub_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr error_publisher_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/haptic_controller/src/haptic_controller.cpp b/haptic_controller/src/haptic_controller.cpp new file mode 100644 index 0000000..203cace --- /dev/null +++ b/haptic_controller/src/haptic_controller.cpp @@ -0,0 +1,404 @@ +// Copyright 2024, ICube Laboratory, University of Strasbourg +// +// 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. + +#include "haptic_controller/haptic_controller.hpp" +// #include "pantograph_mimick_controller/src/pantograph_controller_utils.hpp" +#include + +#include "controller_interface/helpers.hpp" +#include "hardware_interface/loaned_command_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/logging.hpp" +#include "rclcpp/qos.hpp" + +namespace haptic_controller +{ + +const auto kUninitializedValue = std::numeric_limits::quiet_NaN(); +using hardware_interface::HW_IF_POSITION; +using hardware_interface::HW_IF_VELOCITY; +using hardware_interface::HW_IF_EFFORT; + +HapticController::HapticController() +: controller_interface::ControllerInterface(), pantograph_model_() +{ +} + +controller_interface::CallbackReturn HapticController::on_init() +{ + // Implementation of init method + try { + param_listener_ = std::make_shared(get_node()); + } catch (const std::exception & e) { + fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn HapticController::on_configure( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Implementation of on_configure method + auto ret = this->read_parameters(); + if (ret != controller_interface::CallbackReturn::SUCCESS) { + return ret; + } + + pantograph_joint_names_ = { + params_.prefix + "panto_a1", + params_.prefix + "panto_a2", + params_.prefix + "panto_a3", + params_.prefix + "panto_a4", + params_.prefix + "panto_a5", + params_.prefix + "tool_theta_joint", + params_.prefix + "tool_phi_joint", + params_.prefix + "needle_interaction_joint"}; + + // Create subscription to error topic (for testing) + error_subscriber_ = \ + get_node()->create_subscription( + "/angle_error", + rclcpp::SystemDefaultsQoS(), + [this](const std_msgs::msg::Float64::SharedPtr msg) + {rt_command_ptr_.writeFromNonRT(msg);}); + + // Create subscription to marker topic + marker_subscriber_ = \ + get_node()->create_subscription( + "/visualization_marker", 10, + std::bind(&HapticController::marker_callback, this, std::placeholders::_1)); + + // Initialization for motor speed calculation : + last_pos_a1_ = 0.0; + last_pos_a5_ = 0.0; + current_pos_a1_ = 0.0; + current_pos_a5_ = 0.0; + + RCLCPP_INFO(get_node()->get_logger(), "configure successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::InterfaceConfiguration +HapticController::command_interface_configuration() const +{ + // Implementation of command_interface_configuration method + controller_interface::InterfaceConfiguration command_interfaces_config; + command_interfaces_config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + command_interfaces_config.names.push_back( + pantograph_joint_names_[0] + "/" + HW_IF_EFFORT); + command_interfaces_config.names.push_back( + pantograph_joint_names_[4] + "/" + HW_IF_EFFORT); + + + return command_interfaces_config; +} + +controller_interface::InterfaceConfiguration HapticController::state_interface_configuration() +const +{ + return controller_interface::InterfaceConfiguration{ + controller_interface::interface_configuration_type::ALL}; +} + +controller_interface::CallbackReturn HapticController::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + // Implementation of on_activate method + std::vector> + ordered_interfaces; + if ( + !controller_interface::get_ordered_interfaces( + command_interfaces_, command_interface_types_, std::string(""), ordered_interfaces) || + command_interface_types_.size() != ordered_interfaces.size()) + { + RCLCPP_ERROR( + get_node()->get_logger(), "Expected %zu command interfaces, got %zu", + command_interface_types_.size(), ordered_interfaces.size()); + return controller_interface::CallbackReturn::ERROR; + } + + // reset command buffer if a command came through callback when controller was inactive + rt_command_ptr_ = \ + realtime_tools::RealtimeBuffer>(nullptr); + + RCLCPP_INFO(get_node()->get_logger(), "activate successful"); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn HapticController::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + return controller_interface::CallbackReturn::SUCCESS; +} + +double get_value( + const std::unordered_map> & map, + const std::string & name, const std::string & interface_name) +{ + const auto & interfaces_and_values = map.at(name); + const auto interface_and_value = interfaces_and_values.find(interface_name); + if (interface_and_value != interfaces_and_values.cend()) { + return interface_and_value->second; + } else { + return kUninitializedValue; + } +} + +controller_interface::return_type HapticController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) +{ + if (param_listener_->is_old(params_)) { + if (read_parameters() != controller_interface::CallbackReturn::SUCCESS) { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to read parameters during update"); + return controller_interface::return_type::ERROR; + } + } + + for (const auto & state_interface : state_interfaces_) { + name_if_value_mapping_[ + state_interface.get_prefix_name()][state_interface.get_interface_name()] = + state_interface.get_value(); + RCLCPP_DEBUG( + get_node()->get_logger(), "%s/%s: %f\n", state_interface.get_prefix_name().c_str(), + state_interface.get_interface_name().c_str(), state_interface.get_value()); + } + + // Get the previous (active) joint torques + double torque_a1 = get_value(name_if_value_mapping_, pantograph_joint_names_[0], HW_IF_EFFORT); + double torque_a5 = get_value(name_if_value_mapping_, pantograph_joint_names_[4], HW_IF_EFFORT); + + // ========================================================== + // Compute motor rotation speeds using numerical derivation + // ========================================================== + // Step size : + double dt = period.seconds(); + + // Get the current joint positions + double current_pos_a1_ = + get_value(name_if_value_mapping_, pantograph_joint_names_[0], HW_IF_POSITION); + double current_pos_a5_ = + get_value(name_if_value_mapping_, pantograph_joint_names_[4], HW_IF_POSITION); + + double current_vel_a1 = (current_pos_a1_ - last_pos_a1_) / dt; + double current_vel_a5 = (current_pos_a5_ - last_pos_a5_) / dt; + + // Save positions for next calculation + if (current_pos_a1_ != last_pos_a1_) { + last_pos_a1_ = current_pos_a1_; + } + if (current_pos_a5_ != last_pos_a5_) { + last_pos_a5_ = current_pos_a5_; + } + + // Check if positions are close to limit + // Pantograph home position : Q = [q1, q2] = [3.4025, -0.1901] + + // Save positions and velocities to state vectors + Eigen::Vector2d q = Eigen::Vector2d(current_pos_a1_, current_pos_a5_); + Eigen::Vector2d q_dot = Eigen::Vector2d(current_vel_a1, current_vel_a5); + + // ========================================================== + // Compute angle error between desired trajectory (int_marker) + // and current needle orientation + // ========================================================== + + // Get angle error from rt buffer + auto angle_error_msg = rt_command_ptr_.readFromRT(); + // no command received yet + if (!angle_error_msg || !(*angle_error_msg)) { + last_error_ = 0.0; + return controller_interface::return_type::OK; + } + + last_error_ = (*angle_error_msg)->data; + // last_error_ = std::abs(last_error_); // Check if needed + + // Print info in terminal (for testing only) + // RCLCPP_INFO(get_node()->get_logger(), "angle error : %2f ", last_error_); + + // ========================================================== + // Compute joint torques according to force control law + // ========================================================== + Eigen::Vector3d v_needle, v_target, v_proj; + + // Pantograph end effector position : + Eigen::Vector2d P3 = pantograph_model_.fk(q); + + // Compute the pantograph jacobian: + Eigen::Matrix2d J_panto = pantograph_model_.jacobian(q); + + // Pantograph end effector velocities + Eigen::Vector2d P3_dot = J_panto * q_dot; + + // System end effector position : + Eigen::Vector3d PU = pantograph_model_.fk_system(q); + + // Pantograph insertion point + Eigen::Vector3d PI; + // PI << pantograph_model_.PI_x, pantograph_model_.PI_y, pantograph_model_.PI_z; + PI[0] = pantograph_model_.PI_x; + PI[1] = pantograph_model_.PI_y; + PI[2] = pantograph_model_.PI_z; + + // Needle vector (PI to PU) : + v_needle = PU - PI; + + // Needle trajectory vector (PI to Ptarget) : + v_target = marker_pos_ - PI; + + // Projection of Needle vector in trajectory path + v_proj = -v_needle.norm() * std::cos(last_error_) * v_target.normalized(); + + // F_u :Force felt by the user + // F_u Force unit vector + Eigen::Vector3d v_u; + v_u = v_proj - v_needle; + + // Desired guiding force is proportional to error + double F_guide = kp * last_error_; + + // F_guide should not exceed 5 N + if (std::abs(F_guide) > 5) { + F_guide = 5; + } + + // F_u : Force felt by the user at PU + Eigen::Vector3d F_u = F_guide * v_u.normalized(); + + // F_mech : Force that the pantograph needs to apply to create F_u + // Compute intersection point between (v_target,v_needle) plane and (x0,y0) plane + Eigen::Vector3d z_0 = Eigen::Vector3d(0, 0, 1); + double dot1 = z_0.dot(v_target.normalized()); + double t_int = 0; + // Check if dot1 is not null + if (dot1 != 0) { + t_int = -z_0.dot(PI) / dot1; + } else { + t_int = 0; + } + + // Coords of the intersection point + Eigen::Vector3d P_int = PI + t_int * v_target.normalized(); + + // Compute F_mech unit direction vector + Eigen::Vector2d v_mech = (P_int.head(2) - P3).normalized(); + + // Compute alpha angle + double alpha = std::atan2(v_mech[1], v_mech[0]); + + // Compute F_mech magnitude + double norm_F_mech = pantograph_model_.get_panto_force(PU, F_u.norm(), alpha); + + // Force at the pantograph end effector : + // kd_tip : Damping coefficient + // double kd = 0.15 * kp; + Eigen::Vector2d F_mech = norm_F_mech * v_mech - kd_tip * P3_dot; + + // Computation of the active joint torques + Eigen::Vector2d tau = J_panto.transpose() * F_mech; + + // ========================================================== + // Write commands to HW + // ========================================================== + // Limit the torque in the joints for security purposes + double limit = 0.25; // in Nm + + if (abs(tau(0)) > limit) { + RCLCPP_INFO( + get_node()->get_logger(), "Torque command tau_0 too high ! "); + + bool sign_tau_0 = std::signbit(tau(0)); + + if (sign_tau_0) { + tau(0) = limit; + } else { + tau(0) = -limit; + } + } + + if (abs(tau(1)) > limit) { + RCLCPP_INFO( + get_node()->get_logger(), "Torque command tau_1 too high ! "); + + bool sign_tau_1 = std::signbit(tau(1)); + + if (sign_tau_1) { + tau(1) = limit; + } else { + tau(1) = -limit; + } + } + + // Info for debug + RCLCPP_INFO( + get_node()->get_logger(), + "Angle error : %f, F_u (in N) : %.2f, F_mech (in N) : %.2f, Tau (in Nm): %.2f, %.2f", + last_error_, F_u.norm(), F_mech.norm(), tau(0), tau(1)); + + command_interfaces_[0].set_value(tau(0)); + command_interfaces_[1].set_value(tau(1)); + return controller_interface::return_type::OK; +} + +controller_interface::CallbackReturn +HapticController::read_parameters() +{ + if (!param_listener_) { + RCLCPP_ERROR(get_node()->get_logger(), "Error encountered during init"); + return controller_interface::CallbackReturn::ERROR; + } + params_ = param_listener_->get_params(); + + // Update pantograph model parameters + pantograph_model_.set_link_lenghts( + params_.model_parameters.l_a1, + params_.model_parameters.l_a2, + params_.model_parameters.l_a3, + params_.model_parameters.l_a4, + params_.model_parameters.l_a5); + + // Update kp gain : + kp = params_.model_parameters.kp; + // Update kd_tip gain : + kd_tip = params_.model_parameters.kd_tip; + // Update ratio for dampening coef: + ratio = params_.model_parameters.ratio; + return controller_interface::CallbackReturn::SUCCESS; +} + +void haptic_controller::HapticController::marker_callback( + const visualization_msgs::msg::Marker::SharedPtr msg) +{ + if (msg->type == visualization_msgs::msg::Marker::LINE_STRIP && !msg->points.empty()) { + auto points = msg->points; + auto target_point = points[1]; + marker_pos_ << target_point.x, target_point.y, target_point.z; + } else { + // If msg is empty default target points to origin + marker_pos_ << 0.0, 0.0, 0.0; + } +} + +} // namespace haptic_controller + +#include "class_loader/register_macro.hpp" + +CLASS_LOADER_REGISTER_CLASS( + haptic_controller::HapticController, + controller_interface::ControllerInterface) + +// PLUGINLIB_EXPORT_CLASS(haptic_controller::HapticController, controller_interface::ControllerInterface) diff --git a/haptic_controller/src/haptic_controller_parameters.yaml b/haptic_controller/src/haptic_controller_parameters.yaml new file mode 100644 index 0000000..98b8243 --- /dev/null +++ b/haptic_controller/src/haptic_controller_parameters.yaml @@ -0,0 +1,71 @@ +haptic_controller: + prefix: { + type: string, + default_value: "", + description: "Model prefix, none by default.", + } + model_parameters: + kp: { + type: double, + default_value: 1.0, + description: "Proportional gain for force computation", + validation: { + gt<>: [0], + } + } + kd_tip: { + type: double, + default_value: 1.0, + description: "Dampening gain for force computation", + validation: { + gt<>: [0], + } + } + ratio: { + type: double, + default_value: 0.7, + description: "ratio for force dampening ceofficient", + validation: { + gt<>: [0], + } + } + l_a1: { + type: double, + default_value: 0.1, + description: "Length of the first link of the pantograph.", + validation: { + gt<>: [0], + } + } + l_a2: { + type: double, + default_value: 0.165, + description: "Length of the second link of the pantograph.", + validation: { + gt<>: [0], + } + } + l_a3: { + type: double, + default_value: 0.165, + description: "Length of the third link of the pantograph.", + validation: { + gt<>: [0], + } + } + l_a4: { + type: double, + default_value: 0.1, + description: "Length of the fourth link of the pantograph.", + validation: { + gt<>: [0], + } + } + l_a5: { + type: double, + default_value: 0.085, + description: "Length of the fifth link of the pantograph.", + validation: { + gt<>: [0], + } + } diff --git a/nitd.repos b/nitd.repos new file mode 100644 index 0000000..aad3cb7 --- /dev/null +++ b/nitd.repos @@ -0,0 +1,10 @@ +repositories: + external/ethercat_driver_ros2: + type: git + url: https://github.com/ICube-Robotics/ethercat_driver_ros2.git + version: main + + external/needle_pantograph_ros2: + type: git + url: https://github.com/ICube-Robotics/needle_pantograph_ros2.git + version: main