diff --git a/.gitignore b/.gitignore index 02b6441..23da480 100644 --- a/.gitignore +++ b/.gitignore @@ -2,3 +2,10 @@ build/ log/ install/ .vscode/ +3D_Printer/build +3D_Printer/install +3D_Printer/log +3D_printer/src/printer3d_profile_capture/external +3D_printer/src/printer3d_keyence_profile_capture/src/driver +3D_printer/src/printer3d_keyence_profile_capture/launch/__pycache__ +3D_printer/src/printer3d_keyence_data_acquisition_test/launch/__pycache__ \ No newline at end of file diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2abcec7..4fe783d 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -138,7 +138,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.2.4 hooks: - id: codespell args: ['--write-changes'] diff --git a/3D_printer/src/printer3d_driver/CMakeLists.txt b/3D_printer/src/printer3d_driver/CMakeLists.txt new file mode 100644 index 0000000..11cf739 --- /dev/null +++ b/3D_printer/src/printer3d_driver/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_driver) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# Install the python module for this package +# ament_python_install_package(nodes/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install python scripts + +install( + PROGRAMS + nodes/gcode_monitor_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/3D_printer/src/printer3d_driver/nodes/__init__.py b/3D_printer/src/printer3d_driver/nodes/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/3D_printer/src/printer3d_driver/nodes/gcode_monitor_node.py b/3D_printer/src/printer3d_driver/nodes/gcode_monitor_node.py new file mode 100644 index 0000000..aa96aa9 --- /dev/null +++ b/3D_printer/src/printer3d_driver/nodes/gcode_monitor_node.py @@ -0,0 +1,148 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +import time +import serial +from printer3d_msgs.srv import GcodeCommand + + +def openSerialPort(port, baud): + """ + Handle the opening of the serial port and exit if the port haven't been opened. + + Args: + ------- + port (string): supposed to be "COM" plus the number associated to the + baud (int): baudrate of the port communication (commonly 9600, 115200, 250000 ...) + + Returns + ------- + pyserial object: an object allowing to send and receive data through USB + + """ + print('Opening Serial Port...') + try: + s = serial.Serial(port, baud) + if (s.isOpen() is True): + print(port + " has opened successfully.") + return s + else: + print(port + " has failed to open.") + exit() + except Exception: + print("An error occurred while opening " + port) + exit() + + +def removeComment(string): + """ + Remove the comments from a gcode line stored as a string. + + Args + ------- + string (string): gcode line with a possible comment inside + + Returns + ------- + string: input string without comment + + """ + if (string.find(';') == -1): + return string + else: + return string[:string.index(';')] + + +class GcodeMonitorNode(Node): + def __init__(self): + """Gcodemonitornode class constructor.""" + super().__init__('printer_gcode_monitor') + self.get_logger().info("Connecting to device") + self.serialPort = openSerialPort("/dev/ttyACM0", 250000) + self.get_logger().info("device connected") + self.serialPort.write(b"\n\n") + time.sleep(2) + self.gcodeService = self.create_service(GcodeCommand, 'send_gcode', self.execute_gcode_sending) + + def execute_gcode_sending(self, request, response): + """ + Execute this function of the service send_gcode provided by this ROS2 node. + + Args + ------- + request (list of string): list of gcode lines to be sended through the serial communication + response (bool): True + + Returns + ------- + bool: True + + """ + gcode = request.gcode_strings + # self.get_logger().info('Executing Gcode') + feedback = 0.0 + self.serialPort.flushInput() + self.i = 0 + self.total = len(gcode) + self.increment = 0.0 + + for line in gcode: + # feedback publication + feedback = (self.i/self.total)*100 + if feedback - self.increment > 1: + self.increment = feedback + + # Gcode Sending + + line = removeComment(line) + line = line.strip() + self.i += 1 + if (line.isspace() is False and len(line) > 0): + if ' E' in line: + self.lastExtrusionSended = float(line.split('E')[1]) + self.serialPort.write((line + "\n").encode('ascii')) + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + while 'ok' not in grbl_out: + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + + self.serialPort.write(b"M400\n") + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + while 'ok' not in grbl_out: + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + + self.serialPort.write(b"M400\n") + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + while 'ok' not in grbl_out: + grbl_out = self.serialPort.readline() # wait for response from printer + grbl_out = grbl_out.strip().decode('ascii') + + # self.get_logger().info('gcode sending completely finished') + response.validation = True + + return response + + +if __name__ == '__main__': + rclpy.init() + gcode_monitor_node = GcodeMonitorNode() + rclpy.spin(gcode_monitor_node) + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_driver/package.xml b/3D_printer/src/printer3d_driver/package.xml new file mode 100644 index 0000000..55b63bd --- /dev/null +++ b/3D_printer/src/printer3d_driver/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_driver + 0.0.0 + TODO: Package description + gulltor + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_gocator_msgs/CMakeLists.txt b/3D_printer/src/printer3d_gocator_msgs/CMakeLists.txt new file mode 100644 index 0000000..d4faba4 --- /dev/null +++ b/3D_printer/src/printer3d_gocator_msgs/CMakeLists.txt @@ -0,0 +1,29 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_gocator_msgs) + + +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_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/GocatorPTCloud.srv" + "srv/PTCloudTreat.srv" + DEPENDENCIES builtin_interfaces std_msgs sensor_msgs +) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/3D_printer/src/printer3d_gocator_msgs/README.md b/3D_printer/src/printer3d_gocator_msgs/README.md new file mode 100644 index 0000000..e69de29 diff --git a/3D_printer/src/printer3d_gocator_msgs/package.xml b/3D_printer/src/printer3d_gocator_msgs/package.xml new file mode 100644 index 0000000..01f1942 --- /dev/null +++ b/3D_printer/src/printer3d_gocator_msgs/package.xml @@ -0,0 +1,19 @@ + + +printer3d_gocator_msgs +0.0.0 +Messages form communication with GOCATOR systems +m.bednarczyk +TODO: License declaration +ament_cmake +ament_lint_auto +ament_lint_common +std_msgs +sensor_msgs +builtin_interfaces +rosidl_default_generators +rosidl_interface_packages + +ament_cmake + + diff --git a/3D_printer/src/printer3d_gocator_msgs/srv/GocatorPTCloud.srv b/3D_printer/src/printer3d_gocator_msgs/srv/GocatorPTCloud.srv new file mode 100644 index 0000000..f05eb7c --- /dev/null +++ b/3D_printer/src/printer3d_gocator_msgs/srv/GocatorPTCloud.srv @@ -0,0 +1,3 @@ +std_msgs/Empty request +--- +sensor_msgs/PointCloud2 pcloud diff --git a/3D_printer/src/printer3d_gocator_msgs/srv/PTCloudTreat.srv b/3D_printer/src/printer3d_gocator_msgs/srv/PTCloudTreat.srv new file mode 100644 index 0000000..85abec0 --- /dev/null +++ b/3D_printer/src/printer3d_gocator_msgs/srv/PTCloudTreat.srv @@ -0,0 +1,4 @@ +string filenamePointCloud +string order +--- +float32[] output_values \ No newline at end of file diff --git a/3D_printer/src/printer3d_image_capture/CMakeLists.txt b/3D_printer/src/printer3d_image_capture/CMakeLists.txt new file mode 100644 index 0000000..6b97e20 --- /dev/null +++ b/3D_printer/src/printer3d_image_capture/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_image_capture) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# Install the python module for this package +# ament_python_install_package(nodes/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install python scripts + +install( + PROGRAMS + nodes/image_capture_calibration.py + nodes/image_capture_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/3D_printer/src/printer3d_image_capture/nodes/__init__.py b/3D_printer/src/printer3d_image_capture/nodes/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/3D_printer/src/printer3d_image_capture/nodes/image_capture_calibration.py b/3D_printer/src/printer3d_image_capture/nodes/image_capture_calibration.py new file mode 100644 index 0000000..d5665e0 --- /dev/null +++ b/3D_printer/src/printer3d_image_capture/nodes/image_capture_calibration.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +import cv2 + +YPOSPHOTO = 200 +""" + Imagecapturenode class constructor. + + Parameters + ---------- + cam : int + camera number and specifically designed for linux systems. Defaults to 0. + + """ + +class ImageCaptureNode(Node): + def __init__(self, cam=0): + """ + Imagecapturenode class constructor. + + Args: + ---- + cam (int, optional): _description_. Defaults to 0. + + """ + super().__init__('printer_image_capture') + self.cameraNumber = cam + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + def __del__(self): + """Imagecapturenode destructor. Release the camera.""" + self.webcam.release() + + def showVideo(self): + """ + Show the image captured by the camera continuously. Can be stopped by pressing q. + + Returns + ------- + int: returns 0 + + """ + self.webcam = cv2.VideoCapture("/dev/video" + str(self.cameraNumber)) # Be careful with the number after video + while (True): + check, frame = self.webcam.read() + cv2.imshow('frame', frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + self.webcam.release() + cv2.destroyAllWindows() + return 0 + + def sendGcodeSendingRequest(self, gcode): + """ + Ask for the sending of the gcode line "gcode" through the ROS2 service "send_gcode". + + Args: + ---- + gcode: list of gcode lines + + """ + self.req_printer_driver.gcode_strings = gcode + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + self.get_logger().info('gcode sended') + break + + +if __name__ == '__main__': + rclpy.init() + image_capture_node = ImageCaptureNode(0) + gcode = ['G28\n', 'G1 Y'+str(YPOSPHOTO)+' F2400\n'] + image_capture_node.sendGcodeSendingRequest(gcode) + image_capture_node.showVideo() + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_image_capture/nodes/image_capture_node.py b/3D_printer/src/printer3d_image_capture/nodes/image_capture_node.py new file mode 100644 index 0000000..caf9f44 --- /dev/null +++ b/3D_printer/src/printer3d_image_capture/nodes/image_capture_node.py @@ -0,0 +1,46 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node + +from printer3d_msgs.srv import ImageCommand +import cv2 + + +class ImageCaptureNode(Node): + def __init__(self, cam=0): + super().__init__('printer_image_capture') + self.cameraNumber = cam + self.imageService = self.create_service(ImageCommand, 'ask_capture_image', self.takePicture) + + def __del__(self): + self.webcam.release() + + def takePicture(self, request, response): + webcam = cv2.VideoCapture("/dev/video"+str(self.cameraNumber)) # Be careful with the number after video + check, frame = webcam.read() + webcam.release() + cv2.imwrite(filename=request.filename, img=frame) + self.get_logger().info('image acquired : '+request.filename) + response.confirmation = True + return response + + +if __name__ == '__main__': + rclpy.init() + image_capture_node = ImageCaptureNode(0) + rclpy.spin(image_capture_node) + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_image_capture/package.xml b/3D_printer/src/printer3d_image_capture/package.xml new file mode 100644 index 0000000..c897737 --- /dev/null +++ b/3D_printer/src/printer3d_image_capture/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_image_capture + 0.0.0 + TODO: Package description + gulltor + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_keyence_data_acquisition_test/CMakeLists.txt b/3D_printer/src/printer3d_keyence_data_acquisition_test/CMakeLists.txt new file mode 100644 index 0000000..51dc00b --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 3.8) +project(printer3d_keyence_data_acquisition_test) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +# find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# find_package(keyence_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(PROGRAMS + src/nodes/keyence_commands_node.py + + DESTINATION lib/${PROJECT_NAME}) + +# Install launch files. +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}/ +) + +# Install YAML config files. +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +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() + +ament_package() diff --git a/3D_printer/src/printer3d_keyence_data_acquisition_test/LICENSE b/3D_printer/src/printer3d_keyence_data_acquisition_test/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/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/3D_printer/src/printer3d_keyence_data_acquisition_test/config/params.yaml b/3D_printer/src/printer3d_keyence_data_acquisition_test/config/params.yaml new file mode 100644 index 0000000..090cf76 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/config/params.yaml @@ -0,0 +1,10 @@ +keyence_controler: + ros__parameters: + deviceId: 0 + abyIpAddress_1: 192 + abyIpAddress_2: 168 + abyIpAddress_3: 0 + abyIpAddress_4: 1 + wPortNo: 24691 + HighSpeedPortNo: 24692 + \ No newline at end of file diff --git a/3D_printer/src/printer3d_keyence_data_acquisition_test/launch/launch_keyence_commands.py b/3D_printer/src/printer3d_keyence_data_acquisition_test/launch/launch_keyence_commands.py new file mode 100644 index 0000000..049c02e --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/launch/launch_keyence_commands.py @@ -0,0 +1,30 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('printer3d_keyence_data_acquisition_test'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + + return LaunchDescription([ + Node( + package='printer3d_keyence_profile_capture', + namespace='measure_process', + executable='keyence_control_node.py', + name='keyence_control_node' + ), + Node( + package='printer3d_keyence_data_acquisition_test', + namespace='measure_process', + executable='keyence_commands_node.py', + name='keyence_commands_node' + ) + ]) \ No newline at end of file diff --git a/3D_printer/src/printer3d_keyence_data_acquisition_test/package.xml b/3D_printer/src/printer3d_keyence_data_acquisition_test/package.xml new file mode 100644 index 0000000..d1ef5b5 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_keyence_data_acquisition_test + 0.0.0 + ROS2 package to test communication commands. + Mosser Loic + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_keyence_data_acquisition_test/src/nodes/keyence_commands_node.py b/3D_printer/src/printer3d_keyence_data_acquisition_test/src/nodes/keyence_commands_node.py new file mode 100644 index 0000000..d803457 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_data_acquisition_test/src/nodes/keyence_commands_node.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +import time +import ctypes +import sys +import numpy as np +from printer3d_keyence_msgs.srv import KeyenceControlCommands, SaveProfilesCommand + +class KeyenceCommandNode(Node): + def __init__(self): + """KeyenceCommandNode class constructor.""" + super().__init__('printer3d_keyence_data_acquisition_test') + + self.client_keyence_control_service = self.create_client(KeyenceControlCommands, 'keyence_control_commands') + while not self.client_keyence_control_service.wait_for_service(timeout_sec=1.0): + self.get_logger().info('keyence control service not available, waiting again...') + self.req_keyence_control = KeyenceControlCommands.Request() + + self.client_saving_buffer_service = self.create_client(SaveProfilesCommand, 'saving_buffer') + while not self.client_saving_buffer_service.wait_for_service(timeout_sec=1.0): + self.get_logger().info('buffer saving service not available, waiting again...') + self.req_saving_buffer = SaveProfilesCommand.Request() + + def send_command(self,command): + self.req_keyence_control.order = command + self.future_keyence_control = self.client_keyence_control_service.call_async(self.req_keyence_control) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_keyence_control.done(): + self.get_logger().info('command executed') + return 0 + + def ask_for_buffer_saving(self,filename): + self.req_saving_buffer.filenametosave = filename + self.future_buffer_saving = self.client_saving_buffer_service.call_async(self.req_saving_buffer) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_buffer_saving.done(): + self.get_logger().info('command executed') + return 0 + + def sendImageCaptureRequest(self, filename): + self.req_image_capture.filename = filename + self.future_image_capture = self.client_image_capture.call_async(self.req_image_capture) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_image_capture.done(): + self.get_logger().info('capture finished') + break + +if __name__ == '__main__': + rclpy.init() + + keyence_command_node = KeyenceCommandNode() + keyence_command_node.send_command('laser_on') + keyence_command_node.send_command('buffer_flush') + keyence_command_node.get_logger().info("10 secondes pour acquerir des profils") + time.sleep(10) + keyence_command_node.get_logger().info("lancement sauvegarde") + keyence_command_node.send_command('laser_off') + keyence_command_node.ask_for_buffer_saving('/home/gulltor/Ramsai_Robotics/Keyence_data/profiles.npy') + keyence_command_node.send_command('laser_on') + + + + + rclpy.shutdown() \ No newline at end of file diff --git a/3D_printer/src/printer3d_keyence_msgs/CMakeLists.txt b/3D_printer/src/printer3d_keyence_msgs/CMakeLists.txt new file mode 100644 index 0000000..cdb0f23 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_msgs/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_keyence_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces( ${PROJECT_NAME} + "srv/KeyenceControlCommands.srv" + "srv/SaveProfilesCommand.srv" +) + +ament_package() diff --git a/3D_printer/src/printer3d_keyence_msgs/package.xml b/3D_printer/src/printer3d_keyence_msgs/package.xml new file mode 100644 index 0000000..f117516 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_msgs/package.xml @@ -0,0 +1,18 @@ + + +printer3d_keyence_msgs +0.0.0 +Messages for communication with Keyence systems +mosserl +TODO: License declaration +ament_cmake +ament_lint_auto +ament_lint_common +std_msgs +builtin_interfaces +rosidl_default_generators +rosidl_interface_packages + +ament_cmake + + diff --git a/3D_printer/src/printer3d_keyence_msgs/srv/KeyenceControlCommands.srv b/3D_printer/src/printer3d_keyence_msgs/srv/KeyenceControlCommands.srv new file mode 100644 index 0000000..a1526b2 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_msgs/srv/KeyenceControlCommands.srv @@ -0,0 +1,3 @@ +string order +--- +bool validation diff --git a/3D_printer/src/printer3d_keyence_msgs/srv/SaveProfilesCommand.srv b/3D_printer/src/printer3d_keyence_msgs/srv/SaveProfilesCommand.srv new file mode 100644 index 0000000..abfcde3 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_msgs/srv/SaveProfilesCommand.srv @@ -0,0 +1,3 @@ +string filenametosave +--- +bool confirmation diff --git a/3D_printer/src/printer3d_keyence_profile_capture/CMakeLists.txt b/3D_printer/src/printer3d_keyence_profile_capture/CMakeLists.txt new file mode 100644 index 0000000..4d01be7 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(printer3d_keyence_profile_capture) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +# find_package(rclpy REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(rosidl_default_generators REQUIRED) +# find_package(keyence_msgs REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +install(PROGRAMS + src/nodes/keyence_control_node.py + src/driver/PYTHON/LJXAwrap.py + src/driver/PYTHON/libljxacom.so + src/driver/PYTHON/LJXAwrap.py + DESTINATION lib/${PROJECT_NAME}) + +# Install launch files. +install(DIRECTORY +launch +DESTINATION share/${PROJECT_NAME}/ +) + +# Install YAML config files. +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +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() + +ament_package() diff --git a/3D_printer/src/printer3d_keyence_profile_capture/LICENSE b/3D_printer/src/printer3d_keyence_profile_capture/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/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/3D_printer/src/printer3d_keyence_profile_capture/config/params.yaml b/3D_printer/src/printer3d_keyence_profile_capture/config/params.yaml new file mode 100644 index 0000000..090cf76 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/config/params.yaml @@ -0,0 +1,10 @@ +keyence_controler: + ros__parameters: + deviceId: 0 + abyIpAddress_1: 192 + abyIpAddress_2: 168 + abyIpAddress_3: 0 + abyIpAddress_4: 1 + wPortNo: 24691 + HighSpeedPortNo: 24692 + \ No newline at end of file diff --git a/3D_printer/src/printer3d_keyence_profile_capture/launch/launch_keyence_control.py b/3D_printer/src/printer3d_keyence_profile_capture/launch/launch_keyence_control.py new file mode 100644 index 0000000..c8126ca --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/launch/launch_keyence_control.py @@ -0,0 +1,24 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('keyence_controler'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + + return LaunchDescription([ + Node( + package='keyence_controler', + namespace='measure_process', + executable='keyence_control_node.py', + name='keyence_control_node' + ) + ]) \ No newline at end of file diff --git a/3D_printer/src/printer3d_keyence_profile_capture/package.xml b/3D_printer/src/printer3d_keyence_profile_capture/package.xml new file mode 100644 index 0000000..4f24649 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_keyence_profile_capture + 0.0.0 + ROS2 package to control a Keyence LJ-X8080. Will work with all the LJ-X sensors of Keyence. + Mosser Loic + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_keyence_profile_capture/src/nodes/keyence_control_node.py b/3D_printer/src/printer3d_keyence_profile_capture/src/nodes/keyence_control_node.py new file mode 100644 index 0000000..f917604 --- /dev/null +++ b/3D_printer/src/printer3d_keyence_profile_capture/src/nodes/keyence_control_node.py @@ -0,0 +1,473 @@ +#!/usr/bin/env python3 +# 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. + +import LJXAwrap +import rclpy +from rclpy.node import Node +import time +import ctypes +import sys +import numpy as np +from printer3d_keyence_msgs.srv import KeyenceControlCommands,SaveProfilesCommand + +class KeyenceControlNode(Node): + def __init__(self): + """KeyenceControlNode class constructor.""" + super().__init__('printer3d_keyence_profile_capture') + + self.declare_parameter('deviceId', 0) + self.declare_parameter('abyIpAddress_1', 192) + self.declare_parameter('abyIpAddress_2', 168) + self.declare_parameter('abyIpAddress_3', 0) + self.declare_parameter('abyIpAddress_4', 1) + self.declare_parameter('wPortNo', 24691) + self.declare_parameter('HighSpeedPortNo', 24692) + + self.deviceId = self.get_parameter('deviceId').get_parameter_value().integer_value + self.ethernetConfig = LJXAwrap.LJX8IF_ETHERNET_CONFIG() + self.ethernetConfig.abyIpAddress[0] = self.get_parameter('abyIpAddress_1').get_parameter_value().integer_value + self.ethernetConfig.abyIpAddress[1] = self.get_parameter('abyIpAddress_2').get_parameter_value().integer_value + self.ethernetConfig.abyIpAddress[2] = self.get_parameter('abyIpAddress_3').get_parameter_value().integer_value + self.ethernetConfig.abyIpAddress[3] = self.get_parameter('abyIpAddress_4').get_parameter_value().integer_value + self.ethernetConfig.wPortNo = self.get_parameter('wPortNo').get_parameter_value().integer_value + self.HighSpeedPortNo = self.get_parameter('HighSpeedPortNo').get_parameter_value().integer_value + + self.res = LJXAwrap.LJX8IF_EthernetOpen(self.deviceId, self.ethernetConfig) + self.get_logger().info("LJXAwrap.LJX8IF_EthernetOpen:" + str(hex(self.res))) + if self.res != 0: + self.get_logger().info('Failed to connect to the Keyence sensor') + sys.exit() + + # LJ-X 8080 + headmodel = ctypes.create_string_buffer(32) + self.res = LJXAwrap.LJX8IF_GetHeadModel(self.deviceId, headmodel) + self.get_logger().info('LJXAwrap.LJX8IF_GetHeadModel:'+str(hex(self.res))+'='+str(headmodel.value)) + + # LJ-X 8080 serial number + controllerSerial = ctypes.create_string_buffer(16) + headSerial = ctypes.create_string_buffer(16) + res = LJXAwrap.LJX8IF_GetSerialNumber(self.deviceId, + controllerSerial, headSerial) + self.get_logger().info('LJXAwrap.LJX8IF_GetSerialNumber:'+str(hex(self.res))+'='+str(controllerSerial.value)+'='+str(headSerial.value)) + + LJXAwrap.LJX8IF_ClearMemory(self.deviceId) + + self.keyenceControlService = self.create_service(KeyenceControlCommands, 'keyence_control_commands', self.control_service_routine) + self.savingBufferService = self.create_service(SaveProfilesCommand, 'saving_buffer', self.profile_saving_service_routine) + + def select_job(self,job_number): + programNo_get = ctypes.c_ubyte() + self.res = LJXAwrap.LJX8IF_GetActiveProgram(self.deviceId, programNo_get) + if programNo_get.value != job_number: + programNo_set = job_number + res = LJXAwrap.LJX8IF_ChangeActiveProgram(self.deviceId, programNo_set) + return 0 + + def enable_laser(self): + LJXAwrap.LJX8IF_ControlLaser(self.deviceId, 1) + return 0 + + def disable_laser(self): + LJXAwrap.LJX8IF_ControlLaser(self.deviceId, 0) + return 0 + + def get_attention_status(self): + attentionStatus = ctypes.c_ushort() + self.res = LJXAwrap.LJX8IF_GetAttentionStatus(self.deviceId, attentionStatus) + self.get_logger().info("LJXAwrap.LJX8IF_GetAttentionStatus:"+ str(hex(self.res))+"="+ str(bin(attentionStatus.value))) + return 0 + + def ask_for_a_profile(self): + + self.get_logger().info("LJXAwrap.LJX8IF_Trigger : " + str(hex(LJXAwrap.LJX8IF_Trigger(self.deviceId)))) + + # Change according to your controller settings. + xpointNum = 3200 # Number of X points per one profile. + withLumi = 1 # 1: luminance data exists, 0: not exists. + + # Specifies the position, etc. of the profiles to get. + self.req = LJXAwrap.LJX8IF_GET_PROFILE_REQUEST() + self.req.byTargetBank = 0x0 # 0: active bank + self.req.byPositionMode = 0x0 # 0: from current position + self.req.dwGetProfileNo = 0x0 # use when position mode is "POSITION_SPEC" + self.req.byGetProfileCount = 1 # the number of profiles to read. + self.req.byErase = 0 # 0: Do not erase + + self.rsp = LJXAwrap.LJX8IF_GET_PROFILE_RESPONSE() + + self.profinfo = LJXAwrap.LJX8IF_PROFILE_INFO() + + # Calculate the buffer size to store the received profile data. + self.dataSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + self.dataSize += ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_FOOTER) + self.dataSize += ctypes.sizeof(ctypes.c_uint) * xpointNum * (1 + withLumi) + self.dataSize *= self.req.byGetProfileCount + + dataNumIn4byte = int(self.dataSize / ctypes.sizeof(ctypes.c_uint)) + self.profdata = (ctypes.c_int * dataNumIn4byte)() + + # Send command. + self.res = LJXAwrap.LJX8IF_GetProfile(self.deviceId, + self.req, + self.rsp, + self.profinfo, + self.profdata, + self.dataSize) + + self.get_logger().info("LJXAwrap.LJX8IF_GetProfile:" + str(hex(self.res))) + + if self.res != 0: + self.get_logger().info("Failed to get profile.") + sys.exit() + + headerSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + addressOffset_height = int(headerSize / ctypes.sizeof(ctypes.c_uint)) + addressOffset_lumi = addressOffset_height + self.profinfo.wProfileDataCount + + profile_data = [[None, None, None] for i in range(self.profinfo.wProfileDataCount)] + + for i in range(self.profinfo.wProfileDataCount): + # Conver X data to the actual length in millimeters + x_val_mm = (self.profinfo.lXStart + self.profinfo.lXPitch * i) / 100.0 # um + x_val_mm /= 1000.0 # mm + + # Conver Z data to the actual length in millimeters + z_val = self.profdata[addressOffset_height + i] + + if z_val <= -2147483645: # invalid value + z_val_mm = None + else: + z_val_mm = z_val / 100.0 # um + z_val_mm /= 1000.0 # mm + + # Luminance data + lumi_val = self.profdata[addressOffset_lumi + i] + + profile_data[i][0] = x_val_mm + profile_data[i][1] = z_val_mm + profile_data[i][2] = lumi_val + + profile_data = np.array(profile_data) + + return profile_data + + def get_profile_number(self, number): + # Change according to your controller settings. + xpointNum = 3200 # Number of X points per one profile. + withLumi = 1 # 1: luminance data exists, 0: not exists. + + # Specifies the position, etc. of the profiles to get. + self.req = LJXAwrap.LJX8IF_GET_PROFILE_REQUEST() + self.req.byTargetBank = 0x0 # 0: active bank + self.req.byPositionMode = 0x2 # 2: specify position + self.req.dwGetProfileNo = number # use when position mode is "POSITION_SPEC" + self.req.byGetProfileCount = 1 # the number of profiles to read. + self.req.byErase = 0 # 0: Do not erase + + self.rsp = LJXAwrap.LJX8IF_GET_PROFILE_RESPONSE() + + self.profinfo = LJXAwrap.LJX8IF_PROFILE_INFO() + + # Calculate the buffer size to store the received profile data. + self.dataSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + self.dataSize += ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_FOOTER) + self.dataSize += ctypes.sizeof(ctypes.c_uint) * xpointNum * (1 + withLumi) + self.dataSize *= self.req.byGetProfileCount + + dataNumIn4byte = int(self.dataSize / ctypes.sizeof(ctypes.c_uint)) + self.profdata = (ctypes.c_int * dataNumIn4byte)() + + # Send command. + self.res = LJXAwrap.LJX8IF_GetProfile(self.deviceId, + self.req, + self.rsp, + self.profinfo, + self.profdata, + self.dataSize) + if self.res != 0: + self.get_logger().info("Failed to get profile.") + sys.exit() + + headerSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + addressOffset_height = int(headerSize / ctypes.sizeof(ctypes.c_uint)) + addressOffset_lumi = addressOffset_height + self.profinfo.wProfileDataCount + + profile_data = [[None, None, None] for i in range(self.profinfo.wProfileDataCount)] + + for i in range(self.profinfo.wProfileDataCount): + # Conver X data to the actual length in millimeters + x_val_mm = (self.profinfo.lXStart + self.profinfo.lXPitch * i) / 100.0 # um + x_val_mm /= 1000.0 # mm + + # Conver Z data to the actual length in millimeters + z_val = self.profdata[addressOffset_height + i] + + if z_val <= -2147483645: # invalid value + z_val_mm = None + else: + z_val_mm = z_val / 100.0 # um + z_val_mm /= 1000.0 # mm + + # Luminance data + lumi_val = self.profdata[addressOffset_lumi + i] + + profile_data[i][0] = x_val_mm + profile_data[i][1] = z_val_mm + profile_data[i][2] = lumi_val + + profile_data = np.array(profile_data) + + return profile_data + + def what_number_have_last_acquired_profile(self): + # Change according to your controller settings. + xpointNum = 3200 # Number of X points per one profile. + withLumi = 1 # 1: luminance data exists, 0: not exists. + + # Specifies the position, etc. of the profiles to get. + self.req = LJXAwrap.LJX8IF_GET_PROFILE_REQUEST() + self.req.byTargetBank = 0x0 # 0: active bank + self.req.byPositionMode = 0x0 # 0: from current position + self.req.dwGetProfileNo = 0x0 # use when position mode is "POSITION_SPEC" + self.req.byGetProfileCount = 1 # the number of profiles to read. + self.req.byErase = 0 # 0: Do not erase + + self.rsp = LJXAwrap.LJX8IF_GET_PROFILE_RESPONSE() + + self.profinfo = LJXAwrap.LJX8IF_PROFILE_INFO() + + # Calculate the buffer size to store the received profile data. + self.dataSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + self.dataSize += ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_FOOTER) + self.dataSize += ctypes.sizeof(ctypes.c_uint) * xpointNum * (1 + withLumi) + self.dataSize *= self.req.byGetProfileCount + + dataNumIn4byte = int(self.dataSize / ctypes.sizeof(ctypes.c_uint)) + self.profdata = (ctypes.c_int * dataNumIn4byte)() + + # Send command. + self.res = LJXAwrap.LJX8IF_GetProfile(self.deviceId, + self.req, + self.rsp, + self.profinfo, + self.profdata, + self.dataSize) + + self.get_logger().info("LJXAwrap.LJX8IF_GetProfile:" + str(hex(self.res))) + if self.res != 0: + self.get_logger().info("No profile available") + return -1 + else: + return self.rsp.dwGetTopProfileNo + + def get_oldest_profile(self,with_suppression = True): + # Change according to your controller settings. + xpointNum = 3200 # Number of X points per one profile. + withLumi = 1 # 1: luminance data exists, 0: not exists. + + # Specifies the position, etc. of the profiles to get. + self.req = LJXAwrap.LJX8IF_GET_PROFILE_REQUEST() + self.req.byTargetBank = 0x0 # 0: active bank + self.req.byPositionMode = 0x1 # 1: from oldest position + self.req.dwGetProfileNo = 0x0 # use when position mode is "POSITION_SPEC" + self.req.byGetProfileCount = 1 # the number of profiles to read. + self.req.byErase = 1 # 0: Do not erase + + self.rsp = LJXAwrap.LJX8IF_GET_PROFILE_RESPONSE() + + self.profinfo = LJXAwrap.LJX8IF_PROFILE_INFO() + + # Calculate the buffer size to store the received profile data. + self.dataSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + self.dataSize += ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_FOOTER) + self.dataSize += ctypes.sizeof(ctypes.c_uint) * xpointNum * (1 + withLumi) + self.dataSize *= self.req.byGetProfileCount + + dataNumIn4byte = int(self.dataSize / ctypes.sizeof(ctypes.c_uint)) + self.profdata = (ctypes.c_int * dataNumIn4byte)() + + # Send command. + self.res = LJXAwrap.LJX8IF_GetProfile(self.deviceId, + self.req, + self.rsp, + self.profinfo, + self.profdata, + self.dataSize) + + self.get_logger().info("LJXAwrap.LJX8IF_GetProfile:" + str(hex(self.res))) + + if self.res != 0: + self.get_logger().info("Failed to get profile.") + sys.exit() + + headerSize = ctypes.sizeof(LJXAwrap.LJX8IF_PROFILE_HEADER) + addressOffset_height = int(headerSize / ctypes.sizeof(ctypes.c_uint)) + addressOffset_lumi = addressOffset_height + self.profinfo.wProfileDataCount + + profile_data = [[None, None, None] for i in range(self.profinfo.wProfileDataCount)] + + for i in range(self.profinfo.wProfileDataCount): + # Conver X data to the actual length in millimeters + x_val_mm = (self.profinfo.lXStart + self.profinfo.lXPitch * i) / 100.0 # um + x_val_mm /= 1000.0 # mm + + # Conver Z data to the actual length in millimeters + z_val = self.profdata[addressOffset_height + i] + + if z_val <= -2147483645: # invalid value + z_val_mm = None + else: + z_val_mm = z_val / 100.0 # um + z_val_mm /= 1000.0 # mm + + # Luminance data + lumi_val = self.profdata[addressOffset_lumi + i] + + profile_data[i][0] = x_val_mm + profile_data[i][1] = z_val_mm + profile_data[i][2] = lumi_val + + profile_data = np.array(profile_data) + + return profile_data + + def flush_buffer_from_profiles(self): + LJXAwrap.LJX8IF_ClearMemory(self.deviceId) + return 0 + + def control_service_routine(self, request, response): + + command = request.order + + if command == "laser_on": + self.enable_laser() + self.get_logger().info("Laser ON") + response.validation = True + elif command == "laser_off": + self.disable_laser() + self.get_logger().info("Laser OFF") + + response.validation = True + elif command == "buffer_flush": + self.flush_buffer_from_profiles() + self.get_logger().info("Buffer Flushed") + response.validation = True + else: + response.validation = False + + + return response + + def callback_s_a(self, p_header, p_height, p_lumi, luminance_enable, xpointnum, profnum, notify, user): + if (notify == 0) or (notify == 0x10000): + if profnum != 0: + if self.image_available is False: + for i in range(xpointnum * profnum): + self.z_val[i + (xpointnum * profnum * nb_measure)] = p_height[i] + if luminance_enable == 1: + self.lumi_val[i + (xpointnum * profnum * nb_measure)] = p_lumi[i] + self.ysize_acquired = profnum + self.image_available = True + return + + def get_all_available_profiles_fast_transmission(self,nombre): + self.image_available = False + self.ysize_acquired = 0 + self.z_val = [] + self.ysize = nombre + self.lumi_val = [] + + # Initialize Hi-Speed Communication + self.my_callback_s_a = LJXAwrap.LJX8IF_CALLBACK_SIMPLE_ARRAY(self.callback_s_a) + + self.res = LJXAwrap.LJX8IF_InitializeHighSpeedDataCommunicationSimpleArray( + self.deviceId, + self.ethernetConfig, + self.HighSpeedPortNo, + self.my_callback_s_a, + self.ysize, + 0) + + if self.res != 0: + sys.exit() + + # PreStart Hi-Speed Communication + self.req = LJXAwrap.LJX8IF_HIGH_SPEED_PRE_START_REQ() + self.req.bySendPosition = 2 + self.profinfo = LJXAwrap.LJX8IF_PROFILE_INFO() + + self.res = LJXAwrap.LJX8IF_PreStartHighSpeedDataCommunication( + self.deviceId, + self.req, + self.profinfo) + + if self.res != 0: + sys.exit() + + # allocate the memory + self.xsize = self.profinfo.wProfileDataCount + self.z_val = [None] * self.xsize * self.ysize + self.lumi_val = [None] * self.xsize * self.ysize + + # Start Hi-Speed Communication + image_available = False + res = LJXAwrap.LJX8IF_StartHighSpeedDataCommunication(self.deviceId) + print("LJXAwrap.LJX8IF_StartHighSpeedDataCommunication:", hex(res)) + if res != 0: + print("\nExit the program.") + sys.exit() + + LJXAwrap.LJX8IF_StartMeasure(self.deviceId) + + test = True + timeout_sec = 10 + start_time = time.time() + while test == True: + if image_available: + test = False + if time.time() - start_time > timeout_sec: + test = False + + self.res = LJXAwrap.LJX8IF_StopHighSpeedDataCommunication(self.deviceId) + self.res = LJXAwrap.LJX8IF_FinalizeHighSpeedDataCommunication(self.deviceId) + return 0 + + def profile_saving_service_routine(self, request, response): + self.disable_laser() + filename_where_to_save = request.filenametosave + + number_of_profiles = self.what_number_have_last_acquired_profile() + 1 + + # Mode ligne par ligne + #profile_list = [None]*number_of_profiles + #for i in range(0,number_of_profiles): + # profile_list[i] = self.get_profile_number(i) + + self.get_all_available_profiles_fast_transmission(number_of_profiles) + + profiles = np.array(self.z_val) + self.get_logger().info("taile du tableau sauvegarde : " + str(profiles.shape)) + np.save(filename_where_to_save,profiles) + self.enable_laser() + response.confirmation = True + return response + +if __name__ == '__main__': + rclpy.init() + keyence_control_node = KeyenceControlNode() + keyence_control_node.select_job(7) + rclpy.spin(keyence_control_node) + rclpy.shutdown() + diff --git a/3D_printer/src/printer3d_manager/CMakeLists.txt b/3D_printer/src/printer3d_manager/CMakeLists.txt new file mode 100644 index 0000000..b7ca69d --- /dev/null +++ b/3D_printer/src/printer3d_manager/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_manager) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# Install the python module for this package +# ament_python_install_package(nodes/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install python scripts + +install( + PROGRAMS + nodes/printer_node.py + nodes/printer_imageCapture_node.py + nodes/printer_scanner_node.py + nodes/printer_scanner_imageCapture_node.py + nodes/printer_control_test.py + nodes/printer3d_constant.py + nodes/utility.py + nodes/point_cloud2.py + DESTINATION lib/${PROJECT_NAME} +) + +# Install launch files. +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +# Install YAML config files. +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/3D_printer/src/printer3d_manager/config/params.yaml b/3D_printer/src/printer3d_manager/config/params.yaml new file mode 100644 index 0000000..a5afabb --- /dev/null +++ b/3D_printer/src/printer3d_manager/config/params.yaml @@ -0,0 +1,6 @@ +printer3d_manager: + ros__parameters: + history_file_dir: "/home/gulltor/Documents/history/test_packages" + gcode_file_dir: "/home/gulltor/Documents/gcodes/piece_test_base.gcode" + + \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/launch/__pycache__/launch_basic_print.cpython-310.pyc b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_basic_print.cpython-310.pyc new file mode 100644 index 0000000..bfca343 Binary files /dev/null and b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_basic_print.cpython-310.pyc differ diff --git a/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_image_capture_plus_scanner.cpython-310.pyc b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_image_capture_plus_scanner.cpython-310.pyc new file mode 100644 index 0000000..92ac26b Binary files /dev/null and b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_image_capture_plus_scanner.cpython-310.pyc differ diff --git a/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-310.pyc b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-310.pyc new file mode 100644 index 0000000..5d8b2ab Binary files /dev/null and b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-310.pyc differ diff --git a/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-312.pyc b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-312.pyc new file mode 100644 index 0000000..1dc671f Binary files /dev/null and b/3D_printer/src/printer3d_manager/launch/__pycache__/launch_print_with_scan.cpython-312.pyc differ diff --git a/3D_printer/src/printer3d_manager/launch/launch_basic_print.py b/3D_printer/src/printer3d_manager/launch/launch_basic_print.py new file mode 100644 index 0000000..c878822 --- /dev/null +++ b/3D_printer/src/printer3d_manager/launch/launch_basic_print.py @@ -0,0 +1,31 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('printer3d_manager'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + + return LaunchDescription([ + Node( + package='printer3d_driver', + namespace='printing_process', + executable='gcode_monitor_node.py', + name='gcode_monitor_node' + ), + Node( + package='printer3d_manager', + namespace='printing_process', + executable='printer_node.py', + name='printer_control_node', + parameters = [configYaml['printer3d_manager']['ros__parameters']] + ) + ]) \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture.py b/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture.py new file mode 100644 index 0000000..0c43205 --- /dev/null +++ b/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture.py @@ -0,0 +1,36 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('printer3d_manager'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + return LaunchDescription([ + Node( + package='printer3d_driver', + namespace='printing_process', + executable='gcode_monitor_node.py', + name='gcode_monitor_node' + ), + Node( + package='printer3d_image_capture', + namespace='printing_process', + executable='image_capture_node.py', + name='image_capture_node' + ), + Node( + package='printer3d_manager', + namespace='printing_process', + executable='printer_imageCapture_node.py', + name='printer_control_node', + parameters = [configYaml['printer3d_manager']['ros__parameters']] + ) + ]) \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture_plus_scanner.py b/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture_plus_scanner.py new file mode 100644 index 0000000..91cf014 --- /dev/null +++ b/3D_printer/src/printer3d_manager/launch/launch_print_with_image_capture_plus_scanner.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('printer3d_manager'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + + return LaunchDescription([ + Node( + package='printer3d_driver', + namespace='printing_process', + executable='gcode_monitor_node.py', + name='gcode_monitor_node' + ), + Node( + package='printer3d_image_capture', + namespace='printing_process', + executable='image_capture_node.py', + name='image_capture_node' + ), + Node( + package='printer3d_profile_capture', + namespace='printing_process', + executable='gocator_sensor_node', + name='gocator_sensor_node' + ), + Node( + package='printer3d_manager', + namespace='printing_process', + executable='printer_scanner_imageCapture_node.py', + name='printer_control_node', + parameters = [configYaml['printer3d_manager']['ros__parameters']] + ) + ]) \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/launch/launch_print_with_scan.py b/3D_printer/src/printer3d_manager/launch/launch_print_with_scan.py new file mode 100644 index 0000000..f78ab25 --- /dev/null +++ b/3D_printer/src/printer3d_manager/launch/launch_print_with_scan.py @@ -0,0 +1,38 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os +import yaml + +def generate_launch_description(): + config = os.path.join( + get_package_share_directory('printer3d_manager'), + 'config', + 'params.yaml' + ) + + with open(config, 'r') as f: + configYaml = yaml.safe_load(f) + return LaunchDescription([ + Node( + package='printer3d_driver', + namespace='printing_process', + executable='gcode_monitor_node.py', + name='gcode_monitor_node' + ), + Node( + package='printer3d_profile_capture', + namespace='printing_process', + executable='gocator_sensor_node', + name='gocator_sensor' + ), + Node( + package='printer3d_manager', + namespace='printing_process', + executable='printer_scanner_node.py', + name='printer_control_node', + parameters = [configYaml['printer3d_manager']['ros__parameters']] + ) + ]) + + \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/nodes/__init__.py b/3D_printer/src/printer3d_manager/nodes/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/3D_printer/src/printer3d_manager/nodes/point_cloud2.py b/3D_printer/src/printer3d_manager/nodes/point_cloud2.py new file mode 100644 index 0000000..588cd21 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/point_cloud2.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +# 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. + +""" +Serialization of sensor_msgs.PointCloud2 messages. + +Author: Tim Field +ROS2 port by Sebastian Grans (2020) +""" + +import sys + +from collections import namedtuple +import ctypes +import math +import struct + +from sensor_msgs.msg import PointCloud2, PointField + +_DATATYPES = {} +_DATATYPES[PointField.INT8] = ('b', 1) +_DATATYPES[PointField.UINT8] = ('B', 1) +_DATATYPES[PointField.INT16] = ('h', 2) +_DATATYPES[PointField.UINT16] = ('H', 2) +_DATATYPES[PointField.INT32] = ('i', 4) +_DATATYPES[PointField.UINT32] = ('I', 4) +_DATATYPES[PointField.FLOAT32] = ('f', 4) +_DATATYPES[PointField.FLOAT64] = ('d', 8) + + +def read_points(cloud, field_names=None, skip_nans=False, uvs=[]): + assert isinstance(cloud, PointCloud2), 'cloud is not a sensor_msgs.msg.PointCloud2' + fmt = _get_struct_fmt(cloud.is_bigendian, cloud.fields, field_names) + width, height, point_step, row_step, data, isnan = cloud.width, cloud.height, \ + cloud.point_step, cloud.row_step, \ + cloud.data, math.isnan + unpack_from = struct.Struct(fmt).unpack_from + + if skip_nans: + if uvs: + for u, v in uvs: + p = unpack_from(data, (row_step * v) + (point_step * u)) + has_nan = False + for pv in p: + if isnan(pv): + has_nan = True + break + if not has_nan: + yield p + else: + for v in range(height): + offset = row_step * v + for u in range(width): + p = unpack_from(data, offset) + has_nan = False + for pv in p: + if isnan(pv): + has_nan = True + break + if not has_nan: + yield p + offset += point_step + else: + if uvs: + for u, v in uvs: + yield unpack_from(data, (row_step * v) + (point_step * u)) + else: + for v in range(height): + offset = row_step * v + for u in range(width): + yield unpack_from(data, offset) + offset += point_step + + +def read_points_list(cloud, field_names=None, skip_nans=False, uvs=[]): + assert isinstance(cloud, PointCloud2), 'cloud is not a sensor_msgs.msg.PointCloud2' + + if field_names is None: + field_names = [f.name for f in cloud.fields] + + Point = namedtuple("Point", field_names) + + return [Point._make(lines) for lines in read_points(cloud, field_names, skip_nans, uvs)] + + +def create_cloud(header, fields, points): + cloud_struct = struct.Struct(_get_struct_fmt(False, fields)) + + buff = ctypes.create_string_buffer(cloud_struct.size * len(points)) + + point_step, pack_into = cloud_struct.size, cloud_struct.pack_into + offset = 0 + for p in points: + pack_into(buff, offset, *p) + offset += point_step + + return PointCloud2(header=header, + height=1, + width=len(points), + is_dense=False, + is_bigendian=False, + fields=fields, + point_step=cloud_struct.size, + row_step=cloud_struct.size * len(points), + data=buff.raw) + + +def create_cloud_xyz32(header, points): + fields = [PointField(name='x', offset=0, datatype=PointField.FLOAT32, count=1), + PointField(name='y', offset=4, datatype=PointField.FLOAT32, count=1), + PointField(name='z', offset=8, datatype=PointField.FLOAT32, count=1)] + return create_cloud(header, fields, points) + + +def _get_struct_fmt(is_bigendian, fields, field_names=None): + fmt = '>' if is_bigendian else '<' + offset = 0 + for field in (f for f in sorted(fields, key=lambda f: f.offset) if field_names is None or f.name in field_names): + if offset < field.offset: + fmt += 'x' * (field.offset - offset) + offset = field.offset + if field.datatype not in _DATATYPES: + print('Skipping unknown PointField datatype [%d]' % field.datatype, file=sys.stderr) + else: + datatype_fmt, datatype_length = _DATATYPES[field.datatype] + fmt += field.count * datatype_fmt + offset += field.count * datatype_length + + return fmt diff --git a/3D_printer/src/printer3d_manager/nodes/printer3d_constant.py b/3D_printer/src/printer3d_manager/nodes/printer3d_constant.py new file mode 100644 index 0000000..7f48684 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer3d_constant.py @@ -0,0 +1,28 @@ +#!/usr/bin/env python3 +# 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. + +XMIN = 0 +XMAX = 190 + +YMIN = 0 +YMAX = 219 + +ZMIN = 0 +ZMAX = 58 + +GAP = 31.5 + +YPOSPHOTO = 200 +RETRACTION_VALUE = 1 \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/nodes/printer_control_test.py b/3D_printer/src/printer3d_manager/nodes/printer_control_test.py new file mode 100644 index 0000000..aeb1a0c --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer_control_test.py @@ -0,0 +1,214 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +from printer3d_msgs.srv import ImageCommand +from printer3d_gocator_msgs.srv import GocatorPTCloud +from utility import getBordersSimpleGcode, getBordersGcode, work_on_gcode_file +import printer3d_constant +import point_cloud2 as pc2 +import os +import numpy as np + + +class PrinterControlNode(Node): + def __init__(self, historyFilename): + super().__init__('printer_control') + """Initialisation of the printing process workspace""" + self.historyFilename = historyFilename + try: + os.mkdir('/home/gulltor/Ramsai_Robotics/history/'+historyFilename) + except Exception: + pass + + try: + os.mkdir('/home/gulltor/Ramsai_Robotics/history/'+historyFilename+'/gcode') + except Exception: + pass + + try: + os.mkdir('/home/gulltor/Ramsai_Robotics/history/'+historyFilename+'/scan') + except Exception: + pass + + try: + os.mkdir('/home/gulltor/Ramsai_Robotics/history/'+historyFilename+'/photos') + except Exception: + pass + + # access to the image capture service + + self.client_image_capture = self.create_client(ImageCommand, 'ask_capture_image') + while not self.client_image_capture.wait_for_service(timeout_sec=1.0): + self.get_logger().info('image capture service not available, waiting again...') + self.req_image_capture = ImageCommand.Request() + + # access to the profile measure service + + self.client_profile_measure = self.create_client(GocatorPTCloud, 'gocator_get_profile') + while not self.client_profile_measure.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_profile_measure = GocatorPTCloud.Request() + + # access to the printer driver service + + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + self.imageNumber = 0 + self.scanNumber = 0 + + def loadGcode(self, gcodeFilename): + fileFullGcode = open(gcodeFilename) + rawGode = fileFullGcode.readlines() + fileFullGcode.close() + + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersGcode(rawGode) + self.get_logger().info('xMin : '+str(self.xMin)) + self.get_logger().info('xMax : '+str(self.xMax)) + self.get_logger().info('yMin : '+str(self.yMin)) + self.get_logger().info('yMax : '+str(self.yMax)) + self.get_logger().info('zMin : '+str(self.zMin)) + self.get_logger().info('zMax : '+str(self.zMax)) + + assert self.xMin >= printer3d_constant.XMIN + assert self.xMax <= printer3d_constant.XMAX + + assert self.yMin >= printer3d_constant.YMIN + assert self.yMax <= printer3d_constant.YMAX + + assert self.zMin >= printer3d_constant.ZMIN + assert self.zMax <= printer3d_constant.ZMAX + self.gcodeLayers = work_on_gcode_file(rawGode) + + return self.gcodeLayers + + def verifyGcodeBeforeSending(self, gcodeToVerify): + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersSimpleGcode(gcodeToVerify) + testXMin = self.xMin >= printer3d_constant.XMIN + testXMax = self.xMax <= printer3d_constant.XMAX + + testYMin = self.yMin >= printer3d_constant.YMIN + testYMax = self.yMax <= printer3d_constant.YMAX + + testZMin = self.zMin >= printer3d_constant.ZMIN + testZMax = self.zMax <= printer3d_constant.ZMAX + + if testXMin is False or testXMax is False or testYMin is False or testYMax is False or testZMin is False or testZMax is False: + return False + else: + return True + + def scanCurrentLayer(self, printedLayer): + (layerXMin, layerYMin, layerZMin, layerXMax, layerYMax, layerZMax) = getBordersSimpleGcode(printedLayer) + + gcodeScan = [[]] + margin = 20 + + Yinit = layerYMin - printer3d_constant.GAP + Yfinal = layerYMax - printer3d_constant.GAP + step = 0.3 + + gcodeScan[0].append('G1 Z' + str(layerZMax+1)+'\n') + gcodeScan[0].append('G28 X0 Y0\n') + gcodeScan[0].append('G1 Y' + str(Yinit-(margin/2))+' F1200\n') + length = int((Yfinal-Yinit+margin)/step) + + self.get_logger().info('scanning begin :') + self.get_logger().info('Yinit :' + str(Yinit)) + self.get_logger().info('Yfinal :' + str(Yfinal)) + self.get_logger().info('length :' + str(length)) + + for i in range(1, length): + gcodeScan.append([]) + gcodeScan[-1].append('G1 Y'+str(Yinit-(margin/2)+(i*step))+'\n') + surface = [] + for movements in gcodeScan: + self.sendGcodeSendingRequest(movements) + profileLine = self.sendProfileMeasureRequest() + surface.append(profileLine) + np.save('/home/gulltor/Ramsai_Robotics/history/'+self.historyFilename+'/scan/layer_scan_'+str(self.scanNumber)+'.npy', np.array(surface)) + self.scanNumber += 1 + return surface + + def printCurrentLayer(self): + return 0 + + def takeCurrentLayerPhoto(self): + gcodeMoveToPos = ['G28 X0 Y0\n', 'G1 Y'+str(printer3d_constant.YPOSPHOTO)+' F2400\n'] + self.sendGcodeSendingRequest(gcodeMoveToPos) + self.sendImageCaptureRequest('/home/gulltor/Ramsai_Robotics/history/'+self.historyFilename+'/photos/layer_photo_'+str(self.imageNumber)+'.jpg') + self.imageNumber += 1 + return 0 + + def sendImageCaptureRequest(self, filename): + self.req_image_capture.filename = filename + self.future_image_capture = self.client_image_capture.call_async(self.req_image_capture) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_image_capture.done(): + self.get_logger().info('capture finished') + break + + def sendProfileMeasureRequest(self): + profile_line = [] + self.future_profile_measure = self.client_profile_measure.call_async(self.req_profile_measure) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_profile_measure.done(): + try: + self.response = self.future_profile_measure.result() + except Exception as e: + self.get_logger().info(f'Service call failed {e!r}') + else: + pcloud = self.response.pcloud + self.flag_datas_used = True + + gen = pc2.read_points(pcloud, skip_nans=True) + profile = list(gen) + + for points in profile: + profile_line.append([[points[0], 0, points[2]]]) + break + self.get_logger().info('scan finished') + return profile_line + + def sendGcodeSendingRequest(self, gcode): + assert self.verifyGcodeBeforeSending(gcode) is True + + self.req_printer_driver.gcode_strings = gcode + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + self.get_logger().info('gcode sended') + break + + +if __name__ == '__main__': + carriageReturn = ['G28\n'] + rclpy.init() + printer_control_node = PrinterControlNode('test_impression_base') + # printer_control_node.sendGcodeSendingRequest(carriageReturn) + gcode = printer_control_node.loadGcode('/home/gulltor/Ramsai_Robotics/Gcodes/piece_test_base.gcode') + for i in range(0, 3): + printer_control_node.sendGcodeSendingRequest(gcode[i]) + printer_control_node.takeCurrentLayerPhoto() + printer_control_node.scanCurrentLayer(gcode[i]) + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_manager/nodes/printer_imageCapture_node.py b/3D_printer/src/printer3d_manager/nodes/printer_imageCapture_node.py new file mode 100644 index 0000000..ac8fe07 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer_imageCapture_node.py @@ -0,0 +1,177 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +from printer3d_msgs.srv import ImageCommand +from utility import getBordersSimpleGcode, getBordersGcode, work_on_gcode_file, follow_gcode_coordinates +import printer3d_constant +import os +import numpy as np + + +class PrinterControlNode(Node): + def __init__(self): + super().__init__('printer_control') + + """Initialisation of the printing process workspace""" + self.declare_parameter('history_file_dir', './printing_history') + historyFileDir = self.get_parameter('history_file_dir').get_parameter_value().string_value + self.historyFileDir = historyFileDir + self.get_logger().info(historyFileDir) + self.declare_parameter('gcode_file_dir', './piece.gcode') + self.gcodeFileDir = self.get_parameter('gcode_file_dir').get_parameter_value().string_value + self.get_logger().info(self.gcodeFileDir) + + try: + os.mkdir(historyFileDir) + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/gcode') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/scan') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/photos') + except Exception: + pass + + # access to the image capture service + + self.client_image_capture = self.create_client(ImageCommand, 'ask_capture_image') + while not self.client_image_capture.wait_for_service(timeout_sec=1.0): + self.get_logger().info('image capture service not available, waiting again...') + self.req_image_capture = ImageCommand.Request() + + # access to the printer driver service + + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + self.imageNumber = 0 + self.scanNumber = 0 + + self.last_x_value = 0 + self.last_y_value = 0 + self.last_z_value = 0 + self.last_e_value = 0 + + def loadGcode(self, gcodeFilename): + fileFullGcode = open(self.gcodeFileDir) + rawGode = fileFullGcode.readlines() + fileFullGcode.close() + + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersGcode(rawGode) + self.get_logger().info('xMin : ' + str(self.xMin)) + self.get_logger().info('xMax : ' + str(self.xMax)) + self.get_logger().info('yMin : ' + str(self.yMin)) + self.get_logger().info('yMax : ' + str(self.yMax)) + self.get_logger().info('zMin : ' + str(self.zMin)) + self.get_logger().info('zMax : ' + str(self.zMax)) + + assert self.xMin >= printer3d_constant.XMIN + assert self.xMax <= printer3d_constant.XMAX + + assert self.yMin >= printer3d_constant.YMIN + assert self.yMax <= printer3d_constant.YMAX + + assert self.zMin >= printer3d_constant.ZMIN + assert self.zMax <= printer3d_constant.ZMAX + self.gcodeLayers = work_on_gcode_file(rawGode) + + return self.gcodeLayers + + def verifyGcodeBeforeSending(self, gcodeToVerify): + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersSimpleGcode(gcodeToVerify) + testXMin = self.xMin >= printer3d_constant.XMIN + testXMax = self.xMax <= printer3d_constant.XMAX + + testYMin = self.yMin >= printer3d_constant.YMIN + testYMax = self.yMax <= printer3d_constant.YMAX + + testZMin = self.zMin >= printer3d_constant.ZMIN + testZMax = self.zMax <= printer3d_constant.ZMAX + + if testXMin is False or testXMax is False or testYMin is False or testYMax is False or testZMin is False or testZMax is False: + return False + else: + return True + + def printCurrentLayer(self): + return 0 + + def takeCurrentLayerPhoto(self): + gcodeMoveToPos = ['G28 X0 Y0\n', 'G1 Y' + str(printer3d_constant.YPOSPHOTO) + ' F2400\n'] + self.sendGcodeSendingRequest(gcodeMoveToPos) + self.sendImageCaptureRequest(self.historyFileDir+'/photos/layer_photo_'+str(self.imageNumber)+'.jpg') + self.imageNumber += 1 + return 0 + + def sendImageCaptureRequest(self, filename): + self.req_image_capture.filename = filename + self.future_image_capture = self.client_image_capture.call_async(self.req_image_capture) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_image_capture.done(): + self.get_logger().info('capture finished') + break + + def printLastPositions(self): + self.get_logger().info("Last Position : ("+str(self.last_x_value)+", "+str(self.last_y_value)+", "+str(self.last_z_value)+", "+str(self.last_e_value)+")\n") + return 0 + + def sendGcodeSendingRequest(self, gcode): + assert self.verifyGcodeBeforeSending(gcode) is True + + (changed_x,changed_y,changed_z,changed_e) = follow_gcode_coordinates(gcode) + if changed_x != None: + self.last_x_value = changed_x + if changed_y != None: + self.last_y_value = changed_y + if changed_z != None: + self.last_z_value = changed_z + if changed_e != None: + self.last_e_value = changed_e + + self.req_printer_driver.gcode_strings = gcode + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + break + +if __name__ == '__main__': + carriageReturn = ['G28\n'] + rclpy.init() + printer_control_node = PrinterControlNode() + printer_control_node.sendGcodeSendingRequest(carriageReturn) + gcode = printer_control_node.loadGcode() + for i in range(0, len(gcode)-1): + printer_control_node.get_logger.info('lancement de la couche '+str(i+1)+' sur '+str(len(gcode)-1)) + printer_control_node.sendGcodeSendingRequest(gcode[i]) + printer_control_node.get_logger.info('fin de la couche '+str(i+1)+' sur '+str(len(gcode))) + printer_control_node.takeCurrentLayerPhoto() + + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_manager/nodes/printer_node.py b/3D_printer/src/printer3d_manager/nodes/printer_node.py new file mode 100644 index 0000000..4cce6bc --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer_node.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +from utility import getBordersSimpleGcode, getBordersGcode, work_on_gcode_file, follow_gcode_coordinates +import printer3d_constant +import os +import numpy as np + + +class PrinterControlNode(Node): + def __init__(self): + super().__init__('printer_control') + + self.declare_parameter('history_file_dir', './printing_history') + historyFileDir = self.get_parameter('history_file_dir').get_parameter_value().string_value + self.get_logger().info(historyFileDir) + self.declare_parameter('gcode_file_dir', './piece.gcode') + self.gcodeFileDir = self.get_parameter('gcode_file_dir').get_parameter_value().string_value + self.get_logger().info(self.gcodeFileDir) + """Initialisation of the printing process workspace""" + try: + os.mkdir(historyFileDir) + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/gcode') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/scan') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/photos') + except Exception: + pass + + # access to the printer driver service + + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('gcode driver service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + self.imageNumber = 0 + self.scanNumber = 0 + + self.last_x_value = 0 + self.last_y_value = 0 + self.last_z_value = 0 + self.last_e_value = 0 + + def loadGcode(self): + fileFullGcode = open(self.gcodeFileDir) + rawGode = fileFullGcode.readlines() + fileFullGcode.close() + + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersGcode(rawGode) + self.get_logger().info('xMin : ' + str(self.xMin)) + self.get_logger().info('xMax : ' + str(self.xMax)) + self.get_logger().info('yMin : ' + str(self.yMin)) + self.get_logger().info('yMax : ' + str(self.yMax)) + self.get_logger().info('zMin : ' + str(self.zMin)) + self.get_logger().info('zMax : ' + str(self.zMax)) + + assert self.xMin >= printer3d_constant.XMIN + assert self.xMax <= printer3d_constant.XMAX + + assert self.yMin >= printer3d_constant.YMIN + assert self.yMax <= printer3d_constant.YMAX + + assert self.zMin >= printer3d_constant.ZMIN + assert self.zMax <= printer3d_constant.ZMAX + self.gcodeLayers = work_on_gcode_file(rawGode) + + return self.gcodeLayers + + def verifyGcodeBeforeSending(self, gcodeToVerify): + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersSimpleGcode(gcodeToVerify) + testXMin = self.xMin >= printer3d_constant.XMIN + testXMax = self.xMax <= printer3d_constant.XMAX + + testYMin = self.yMin >= printer3d_constant.YMIN + testYMax = self.yMax <= printer3d_constant.YMAX + + testZMin = self.zMin >= printer3d_constant.ZMIN + testZMax = self.zMax <= printer3d_constant.ZMAX + + if testXMin is False or testXMax is False or testYMin is False or testYMax is False or testZMin is False or testZMax is False: + return False + else: + return True + + + def printCurrentLayer(self): + return 0 + + def printLastPositions(self): + self.get_logger().info("Last Position : ("+str(self.last_x_value)+", "+str(self.last_y_value)+", "+str(self.last_z_value)+", "+str(self.last_e_value)+")\n") + return 0 + + def sendGcodeSendingRequest(self, gcode): + assert self.verifyGcodeBeforeSending(gcode) is True + + (changed_x,changed_y,changed_z,changed_e) = follow_gcode_coordinates(gcode) + if changed_x != None: + self.last_x_value = changed_x + if changed_y != None: + self.last_y_value = changed_y + if changed_z != None: + self.last_z_value = changed_z + if changed_e != None: + self.last_e_value = changed_e + + self.req_printer_driver.gcode_strings = gcode + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + break + +if __name__ == '__main__': + carriageReturn = ['G28\n'] + rclpy.init() + printer_control_node = PrinterControlNode() + printer_control_node.sendGcodeSendingRequest(carriageReturn) + gcode = printer_control_node.loadGcode() + for i in range(0, len(gcode)-1): + printer_control_node.get_logger().info('lancement de la couche '+str(i+1)+' sur '+str(len(gcode)-1)) + printer_control_node.sendGcodeSendingRequest(gcode[i]) + printer_control_node.printLastPositions() + printer_control_node.get_logger().info('fin de la couche '+str(i+1)+' sur '+str(len(gcode))) + + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_manager/nodes/printer_scanner_imageCapture_node.py b/3D_printer/src/printer3d_manager/nodes/printer_scanner_imageCapture_node.py new file mode 100644 index 0000000..9ebd558 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer_scanner_imageCapture_node.py @@ -0,0 +1,263 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +from printer3d_msgs.srv import ImageCommand +from printer3d_gocator_msgs.srv import GocatorPTCloud +from utility import getBordersSimpleGcode, getBordersGcode, work_on_gcode_file, follow_gcode_coordinates +import printer3d_constant +import point_cloud2 as pc2 +import os +import numpy as np + + +class PrinterControlNode(Node): + def __init__(self): + super().__init__('printer_control') + + """Initialisation of the printing process workspace""" + self.declare_parameter('history_file_dir', './printing_history') + historyFileDir = self.get_parameter('history_file_dir').get_parameter_value().string_value + self.historyFileDir = historyFileDir + self.get_logger().info(historyFileDir) + self.declare_parameter('gcode_file_dir', './piece.gcode') + self.gcodeFileDir = self.get_parameter('gcode_file_dir').get_parameter_value().string_value + self.get_logger().info(self.gcodeFileDir) + + try: + os.mkdir(historyFileDir) + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/gcode') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/scan') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/photos') + except Exception: + pass + + # access to the image capture service + + self.client_image_capture = self.create_client(ImageCommand, 'ask_capture_image') + while not self.client_image_capture.wait_for_service(timeout_sec=1.0): + self.get_logger().info('image capture service not available, waiting again...') + self.req_image_capture = ImageCommand.Request() + + # access to the profile measure service + + self.client_profile_measure = self.create_client(GocatorPTCloud, 'gocator_get_profile') + while not self.client_profile_measure.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_profile_measure = GocatorPTCloud.Request() + + # access to the printer driver service + + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + self.imageNumber = 0 + self.scanNumber = 0 + + self.last_x_value = 0 + self.last_y_value = 0 + self.last_z_value = 0 + self.last_e_value = 0 + + def takeCurrentLayerPhoto(self): + gcodeMoveToPos = ['G28 X0 Y0\n', 'G1 Y' + str(printer3d_constant.YPOSPHOTO) + ' F2400\n'] + self.sendGcodeSendingRequest(gcodeMoveToPos,True,False) + self.sendImageCaptureRequest(self.historyFileDir+'/photos/layer_photo_'+str(self.imageNumber)+'.jpg') + self.imageNumber += 1 + return 0 + + def sendImageCaptureRequest(self, filename): + self.req_image_capture.filename = filename + self.future_image_capture = self.client_image_capture.call_async(self.req_image_capture) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_image_capture.done(): + self.get_logger().info('capture finished') + break + + def loadGcode(self): + fileFullGcode = open(self.gcodeFileDir) + rawGode = fileFullGcode.readlines() + fileFullGcode.close() + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersGcode(rawGode) + # self.get_logger().info('xMin : ' + str(self.xMin)) + # self.get_logger().info('xMax : ' + str(self.xMax)) + # self.get_logger().info('yMin : ' + str(self.yMin)) + # self.get_logger().info('yMax : ' + str(self.yMax)) + # self.get_logger().info('zMin : ' + str(self.zMin)) + # self.get_logger().info('zMax : ' + str(self.zMax)) + + assert self.xMin >= printer3d_constant.XMIN + assert self.xMax <= printer3d_constant.XMAX + + assert self.yMin >= printer3d_constant.YMIN + assert self.yMax <= printer3d_constant.YMAX + + assert self.zMin >= printer3d_constant.ZMIN + assert self.zMax <= printer3d_constant.ZMAX + self.gcodeLayers = work_on_gcode_file(rawGode) + + return self.gcodeLayers + + def verifyGcodeBeforeSending(self, gcodeToVerify): + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersSimpleGcode(gcodeToVerify) + testXMin = self.xMin >= printer3d_constant.XMIN + testXMax = self.xMax <= printer3d_constant.XMAX + + testYMin = self.yMin >= printer3d_constant.YMIN + testYMax = self.yMax <= printer3d_constant.YMAX + + testZMin = self.zMin >= printer3d_constant.ZMIN + testZMax = self.zMax <= printer3d_constant.ZMAX + + if testXMin is False or testXMax is False or testYMin is False or testYMax is False or testZMin is False or testZMax is False: + return False + else: + return True + + def scanCurrentLayer(self, printedLayer): + (layerXMin, layerYMin, layerZMin, layerXMax, layerYMax, layerZMax) = getBordersSimpleGcode(printedLayer) + + gcodeScan = [[]] + margin = 25 + + Yinit = layerYMin - printer3d_constant.GAP + Yfinal = layerYMax - printer3d_constant.GAP + step = 0.3 + + gcodeScan[0].append('G1 Z' + str(layerZMax+1)+'\n') + gcodeScan[0].append('G28 X0 Y0\n') + gcodeScan[0].append('G1 Y' + str(Yinit-(margin/2)) + ' F1200\n') + length = int((Yfinal-Yinit+margin)/step) + + self.get_logger().info('scanning begin') + # self.get_logger().info('Yinit :' + str(Yinit)) + # self.get_logger().info('Yfinal :' + str(Yfinal)) + # self.get_logger().info('length :' + str(length)) + + for i in range(1, length): + gcodeScan.append([]) + gcodeScan[-1].append('G1 Y' + str(Yinit-(margin/2)+(i*step))+'\n') + surface = [] + iterator = len(gcodeScan) + for movements in gcodeScan: + if iterator == len(gcodeScan)-1: + self.sendGcodeSendingRequest(movements, False, True) + else: + self.sendGcodeSendingRequest(movements) + profileLine = self.sendProfileMeasureRequest() + surface.append(profileLine) + iterator += 1 + np.save(self.historyFileDir+'/scan/layer_scan_'+str(self.scanNumber)+'.npy', np.array(surface)) + self.scanNumber += 1 + return surface + + def printCurrentLayer(self): + return 0 + + def sendProfileMeasureRequest(self): + profile_line = [] + self.future_profile_measure = self.client_profile_measure.call_async(self.req_profile_measure) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_profile_measure.done(): + try: + self.response = self.future_profile_measure.result() + except Exception as e: + self.get_logger().info(f'Service call failed {e!r}') + else: + pcloud = self.response.pcloud + self.flag_datas_used = True + + gen = pc2.read_points(pcloud, skip_nans=True) + profile = list(gen) + + for points in profile: + profile_line.append([[points[0], 0, points[2]]]) + break + return profile_line + + def printLastPositions(self): + self.get_logger().info("Last Position : ("+str(self.last_x_value)+", "+str(self.last_y_value)+", "+str(self.last_z_value)+", "+str(self.last_e_value)+")\n") + return 0 + + def sendGcodeSendingRequest(self, gcode, with_retraction = False, with_extrusion = False): + assert self.verifyGcodeBeforeSending(gcode) is True + + (changed_x,changed_y,changed_z,changed_e) = follow_gcode_coordinates(gcode) + if changed_x != None: + self.last_x_value = changed_x + if changed_y != None: + self.last_y_value = changed_y + if changed_z != None: + self.last_z_value = changed_z + if changed_e != None: + self.last_e_value = changed_e + + if with_retraction: + self.get_logger().info("retraction enclenchée") + self.req_printer_driver.gcode_strings = ['G1 F60 E'+str(self.last_e_value-printer3d_constant.RETRACTION_VALUE)+'\n']+gcode + elif with_extrusion: + self.get_logger().info("extrusion enclenchée") + self.req_printer_driver.gcode_strings = gcode+['G1 F60 E'+str(self.last_e_value)+'\n'] + else: + self.req_printer_driver.gcode_strings = gcode + + fichier_sauvegarde = open(self.historyFileDir+'/gcode/sendedGcode.gcode','a') + for lines in self.req_printer_driver.gcode_strings: + fichier_sauvegarde.write(lines) + fichier_sauvegarde.close() + + + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + break + +if __name__ == '__main__': + carriageReturn = ['G28\n'] + rclpy.init() + printer_control_node = PrinterControlNode() + printer_control_node.sendGcodeSendingRequest(carriageReturn) + gcode = printer_control_node.loadGcode() + for i in range(0, len(gcode)-1): + printer_control_node.get_logger().info('lancement de la couche '+str(i+1)+' sur '+str(len(gcode)-1)) + printer_control_node.sendGcodeSendingRequest(gcode[i]) + printer_control_node.printLastPositions() + printer_control_node.get_logger().info('fin de la couche '+str(i+1)+' sur '+str(len(gcode))) + printer_control_node.takeCurrentLayerPhoto() + if i == 0: + printer_control_node.scanCurrentLayer(gcode[i+1]) + else: + printer_control_node.scanCurrentLayer(gcode[i]) + + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_manager/nodes/printer_scanner_node.py b/3D_printer/src/printer3d_manager/nodes/printer_scanner_node.py new file mode 100644 index 0000000..cbee0e1 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/printer_scanner_node.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +from printer3d_gocator_msgs.srv import GocatorPTCloud +from utility import getBordersSimpleGcode, getBordersGcode, work_on_gcode_file, follow_gcode_coordinates +import printer3d_constant +import point_cloud2 as pc2 +import os +import numpy as np + + +class PrinterControlNode(Node): + def __init__(self): + super().__init__('printer_control') + + """Initialisation of the printing process workspace""" + self.declare_parameter('history_file_dir', './printing_history') + historyFileDir = self.get_parameter('history_file_dir').get_parameter_value().string_value + self.historyFileDir = historyFileDir + self.get_logger().info(historyFileDir) + self.declare_parameter('gcode_file_dir', './piece.gcode') + self.gcodeFileDir = self.get_parameter('gcode_file_dir').get_parameter_value().string_value + self.get_logger().info(self.gcodeFileDir) + + try: + os.mkdir(historyFileDir) + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/gcode') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/scan') + except Exception: + pass + + try: + os.mkdir(historyFileDir+'/photos') + except Exception: + pass + + # access to the printer driver service + + self.client_printer_driver = self.create_client(GcodeCommand, 'send_gcode') + while not self.client_printer_driver.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_printer_driver = GcodeCommand.Request() + + # access to the profile measure service + + self.client_profile_measure = self.create_client(GocatorPTCloud, 'gocator_get_profile') + while not self.client_profile_measure.wait_for_service(timeout_sec=1.0): + self.get_logger().info('profile measure service not available, waiting again...') + self.req_profile_measure = GocatorPTCloud.Request() + + self.imageNumber = 0 + self.scanNumber = 0 + + self.last_x_value = 0 + self.last_y_value = 0 + self.last_z_value = 0 + self.last_e_value = 0 + + def loadGcode(self): + fileFullGcode = open(self.gcodeFileDir) + rawGode = fileFullGcode.readlines() + fileFullGcode.close() + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersGcode(rawGode) + # self.get_logger().info('xMin : ' + str(self.xMin)) + # self.get_logger().info('xMax : ' + str(self.xMax)) + # self.get_logger().info('yMin : ' + str(self.yMin)) + # self.get_logger().info('yMax : ' + str(self.yMax)) + # self.get_logger().info('zMin : ' + str(self.zMin)) + # self.get_logger().info('zMax : ' + str(self.zMax)) + + assert self.xMin >= printer3d_constant.XMIN + assert self.xMax <= printer3d_constant.XMAX + + assert self.yMin >= printer3d_constant.YMIN + assert self.yMax <= printer3d_constant.YMAX + + assert self.zMin >= printer3d_constant.ZMIN + assert self.zMax <= printer3d_constant.ZMAX + self.gcodeLayers = work_on_gcode_file(rawGode) + + return self.gcodeLayers + + def verifyGcodeBeforeSending(self, gcodeToVerify): + (self.xMin, self.yMin, self.zMin, self.xMax, self.yMax, self.zMax) = getBordersSimpleGcode(gcodeToVerify) + testXMin = self.xMin >= printer3d_constant.XMIN + testXMax = self.xMax <= printer3d_constant.XMAX + + testYMin = self.yMin >= printer3d_constant.YMIN + testYMax = self.yMax <= printer3d_constant.YMAX + + testZMin = self.zMin >= printer3d_constant.ZMIN + testZMax = self.zMax <= printer3d_constant.ZMAX + + if testXMin is False or testXMax is False or testYMin is False or testYMax is False or testZMin is False or testZMax is False: + return False + else: + return True + + def scanCurrentLayer(self, printedLayer): + (layerXMin, layerYMin, layerZMin, layerXMax, layerYMax, layerZMax) = getBordersSimpleGcode(printedLayer) + + gcodeScan = [[]] + margin = 25 + + Yinit = layerYMin - printer3d_constant.GAP + Yfinal = layerYMax - printer3d_constant.GAP + step = 0.3 + + gcodeScan[0].append('G1 Z' + str(layerZMax+1)+'\n') + gcodeScan[0].append('G28 X0 Y0\n') + gcodeScan[0].append('G1 Y' + str(Yinit-(margin/2)) + ' F1200\n') + length = int((Yfinal-Yinit+margin)/step) + + self.get_logger().info('scanning begin') + # self.get_logger().info('Yinit :' + str(Yinit)) + # self.get_logger().info('Yfinal :' + str(Yfinal)) + # self.get_logger().info('length :' + str(length)) + + for i in range(1, length): + gcodeScan.append([]) + gcodeScan[-1].append('G1 Y' + str(Yinit-(margin/2)+(i*step))+'\n') + surface = [] + iterator = 0 + for movements in gcodeScan: + if iterator == 0: + self.sendGcodeSendingRequest(movements, True) + else: + self.sendGcodeSendingRequest(movements) + profileLine = self.sendProfileMeasureRequest() + surface.append(profileLine) + iterator += 1 + np.save(self.historyFileDir+'/scan/layer_scan_'+str(self.scanNumber)+'.npy', np.array(surface)) + self.scanNumber += 1 + return surface + + def printCurrentLayer(self): + return 0 + + def sendProfileMeasureRequest(self): + profile_line = [] + self.future_profile_measure = self.client_profile_measure.call_async(self.req_profile_measure) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_profile_measure.done(): + try: + self.response = self.future_profile_measure.result() + except Exception as e: + self.get_logger().info(f'Service call failed {e!r}') + else: + pcloud = self.response.pcloud + self.flag_datas_used = True + + gen = pc2.read_points(pcloud, skip_nans=True) + profile = list(gen) + + for points in profile: + profile_line.append([[points[0], 0, points[2]]]) + break + return profile_line + + def printLastPositions(self): + self.get_logger().info("Last Position : ("+str(self.last_x_value)+", "+str(self.last_y_value)+", "+str(self.last_z_value)+", "+str(self.last_e_value)+")\n") + return 0 + + def sendGcodeSendingRequest(self, gcode, with_retraction = False): + assert self.verifyGcodeBeforeSending(gcode) is True + + (changed_x,changed_y,changed_z,changed_e) = follow_gcode_coordinates(gcode) + if changed_x != None: + self.last_x_value = changed_x + if changed_y != None: + self.last_y_value = changed_y + if changed_z != None: + self.last_z_value = changed_z + if changed_e != None: + self.last_e_value = changed_e + + if with_retraction: + self.get_logger().info("retraction enclenchée") + self.req_printer_driver.gcode_strings = ['G1 F60 E'+str(self.last_e_value-printer3d_constant.RETRACTION_VALUE)+'\n']+gcode+['G1 F60 E'+str(self.last_e_value)+'\n'] + else: + self.req_printer_driver.gcode_strings = gcode + + fichier_sauvegarde = open(self.historyFileDir+'/gcode/sendedGcode.gcode','a') + for lines in self.req_printer_driver.gcode_strings: + fichier_sauvegarde.write(lines) + fichier_sauvegarde.close() + + + self.future_printer_driver = self.client_printer_driver.call_async(self.req_printer_driver) + while rclpy.ok(): + rclpy.spin_once(self) + if self.future_printer_driver.done(): + break + +if __name__ == '__main__': + carriageReturn = ['G28\n'] + rclpy.init() + printer_control_node = PrinterControlNode() + printer_control_node.sendGcodeSendingRequest(carriageReturn) + gcode = printer_control_node.loadGcode() + for i in range(0, len(gcode)-1): + printer_control_node.get_logger().info('lancement de la couche '+str(i+1)+' sur '+str(len(gcode)-1)) + printer_control_node.sendGcodeSendingRequest(gcode[i]) + printer_control_node.printLastPositions() + printer_control_node.get_logger().info('fin de la couche '+str(i+1)+' sur '+str(len(gcode)-1)) + printer_control_node.scanCurrentLayer(gcode[i]) + + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_manager/nodes/utility.py b/3D_printer/src/printer3d_manager/nodes/utility.py new file mode 100644 index 0000000..ad63631 --- /dev/null +++ b/3D_printer/src/printer3d_manager/nodes/utility.py @@ -0,0 +1,142 @@ +# 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. + + +def work_on_gcode_file(sequence): + + delimiter = ';LAYER:' + final_delimiter = '; Default end code' + layers = [[]] + + for line in sequence: + if (delimiter not in line) and (final_delimiter not in line): + if line[0] != ';': + layers[-1].append(line) + else: + layers.append([]) + + return layers + + +def getBordersGcode(sequence): + zMax = 0 + xMax = 0 + yMax = 0 + zMin = 2000 + yMin = 2000 + xMin = 2000 + + # remove initialisation + + completeSequence = work_on_gcode_file(sequence) + innerSequence = [] + for i in range(1, len(completeSequence)-1): + innerSequence = innerSequence+completeSequence[i] + + for line in innerSequence: + if 'Z' in line and ';' not in line: + z = line.split('Z')[1] + z = z.split(' ')[0] + z = z.split('\n')[0] + z = float(z) + if z > zMax: + zMax = z + if z < zMin: + zMin = z + if 'Y' in line and ';' not in line: + y = line.split('Y')[1] + y = y.split(' ')[0] + y = y.split('\n')[0] + y = float(y) + if y > yMax: + yMax = y + if y < yMin: + yMin = y + if 'X' in line and ';' not in line: + x = line.split('X')[1] + x = x.split(' ')[0] + x = x.split('\n')[0] + x = float(x) + if x > xMax: + xMax = x + if x < xMin: + xMin = x + return (xMin, yMin, zMin, xMax, yMax, zMax) + + +def getBordersSimpleGcode(simpleGcode): + zMax = 0 + xMax = 0 + yMax = 0 + zMin = 2000 + yMin = 2000 + xMin = 2000 + + for line in simpleGcode: + if 'Z' in line and ';' not in line: + z = line.split('Z')[1] + z = z.split(' ')[0] + z = z.split('\n')[0] + z = float(z) + if z > zMax: + zMax = z + if z < zMin: + zMin = z + if 'Y' in line and ';' not in line: + y = line.split('Y')[1] + y = y.split(' ')[0] + y = y.split('\n')[0] + y = float(y) + if y > yMax: + yMax = y + if y < yMin: + yMin = y + if 'X' in line and ';' not in line: + x = line.split('X')[1] + x = x.split(' ')[0] + x = x.split('\n')[0] + x = float(x) + if x > xMax: + xMax = x + if x < xMin: + xMin = x + return (xMin, yMin, zMin, xMax, yMax, zMax) + +def follow_gcode_coordinates(gcodeLines): + lastX = None + lastY = None + lastZ = None + lastE = None + for line in gcodeLines: + if 'X' in line: + splittedLine = line.split(' ') + for elements in splittedLine: + if elements[0] == 'X': + lastX = float(elements[1:]) + if 'Y' in line: + splittedLine = line.split(' ') + for elements in splittedLine: + if elements[0] == 'Y': + lastY = float(elements[1:]) + if 'Z' in line: + splittedLine = line.split(' ') + for elements in splittedLine: + if elements[0] == 'Z': + lastZ = float(elements[1:]) + if 'E' in line: + splittedLine = line.split(' ') + for elements in splittedLine: + if elements[0] == 'E': + lastE = float(elements[1:]) + return (lastX,lastY,lastZ,lastE) \ No newline at end of file diff --git a/3D_printer/src/printer3d_manager/package.xml b/3D_printer/src/printer3d_manager/package.xml new file mode 100644 index 0000000..84d5081 --- /dev/null +++ b/3D_printer/src/printer3d_manager/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_manager + 0.0.0 + TODO: Package description + gulltor + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_msgs/CMakeLists.txt b/3D_printer/src/printer3d_msgs/CMakeLists.txt new file mode 100644 index 0000000..88bce6c --- /dev/null +++ b/3D_printer/src/printer3d_msgs/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_msgs) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +rosidl_generate_interfaces( ${PROJECT_NAME} + "srv/ProfileCommand.srv" + "srv/GcodeCommand.srv" + "srv/ImageCommand.srv" +) + +ament_package() diff --git a/3D_printer/src/printer3d_msgs/package.xml b/3D_printer/src/printer3d_msgs/package.xml new file mode 100644 index 0000000..5859e20 --- /dev/null +++ b/3D_printer/src/printer3d_msgs/package.xml @@ -0,0 +1,19 @@ + + +printer3d_msgs +0.0.0 +Messages form communication with GOCATOR systems +m.bednarczyk +TODO: License declaration +ament_cmake +ament_lint_auto +ament_lint_common +std_msgs +sensor_msgs +builtin_interfaces +rosidl_default_generators +rosidl_interface_packages + +ament_cmake + + diff --git a/3D_printer/src/printer3d_msgs/srv/GcodeCommand.srv b/3D_printer/src/printer3d_msgs/srv/GcodeCommand.srv new file mode 100644 index 0000000..c1509c0 --- /dev/null +++ b/3D_printer/src/printer3d_msgs/srv/GcodeCommand.srv @@ -0,0 +1,3 @@ +string[] gcode_strings +--- +bool validation diff --git a/3D_printer/src/printer3d_msgs/srv/ImageCommand.srv b/3D_printer/src/printer3d_msgs/srv/ImageCommand.srv new file mode 100644 index 0000000..8e5725f --- /dev/null +++ b/3D_printer/src/printer3d_msgs/srv/ImageCommand.srv @@ -0,0 +1,3 @@ +string filename +--- +bool confirmation diff --git a/3D_printer/src/printer3d_msgs/srv/ProfileCommand.srv b/3D_printer/src/printer3d_msgs/srv/ProfileCommand.srv new file mode 100644 index 0000000..d1db3c3 --- /dev/null +++ b/3D_printer/src/printer3d_msgs/srv/ProfileCommand.srv @@ -0,0 +1,6 @@ +float64 y_min +float64 y_max +float64 z_upper +float64 step +--- +bool validation diff --git a/3D_printer/src/printer3d_pointCloud_processing/CMakeLists.txt b/3D_printer/src/printer3d_pointCloud_processing/CMakeLists.txt new file mode 100755 index 0000000..2285557 --- /dev/null +++ b/3D_printer/src/printer3d_pointCloud_processing/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.5) +project(printer3d_point_cloud_processing) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) + +# Install the python module for this package +# ament_python_install_package(nodes/) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +# Install python scripts + +install( + PROGRAMS + nodes/point_cloud_processing_node.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/3D_printer/src/printer3d_pointCloud_processing/nodes/__init__.py b/3D_printer/src/printer3d_pointCloud_processing/nodes/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/3D_printer/src/printer3d_pointCloud_processing/nodes/constants.py b/3D_printer/src/printer3d_pointCloud_processing/nodes/constants.py new file mode 100644 index 0000000..f596e65 --- /dev/null +++ b/3D_printer/src/printer3d_pointCloud_processing/nodes/constants.py @@ -0,0 +1,2 @@ +ORDRE_INITIALISATION = 0 +ORDRE_CALC_M_S = 1 \ No newline at end of file diff --git a/3D_printer/src/printer3d_pointCloud_processing/nodes/mask_creation_utility.py b/3D_printer/src/printer3d_pointCloud_processing/nodes/mask_creation_utility.py new file mode 100644 index 0000000..0a6edad --- /dev/null +++ b/3D_printer/src/printer3d_pointCloud_processing/nodes/mask_creation_utility.py @@ -0,0 +1,138 @@ +import matplotlib.pyplot as plt +import numpy as np +from stl import mesh +from math import sqrt + +def intersection_segment_plan(p1, p2, a, b, c, d): + """Calcule l'intersection entre un segment [p1, p2] et le plan ax + by + cz + d = 0. + Retourne le point d'intersection s'il existe, sinon None.""" + # Vecteur directeur de l'arête + u = p2 - p1 + # Produit scalaire avec la normale du plan + denom = np.dot(u, np.array([a, b, c])) + if abs(denom) < 1e-10: # Arête parallèle au plan + return None + # Paramètre t de l'intersection + t = -(a * p1[0] + b * p1[1] + c * p1[2] + d) / denom + if 0 <= t <= 1: # Intersection dans le segment + return p1 + t * u + return None + +def find_neighbor(segment, triangle_intersions, tolerance): + neighbor = [] + neighbor_indices = [] + for j in range(0,len(triangle_intersions)): + test_P1_segment_P1_intersection = sqrt((segment[0][0]-triangle_intersions[j][0][0])**2+(segment[0][1]-triangle_intersions[j][0][1])**2) < tolerance + test_P1_segment_P2_intersection = sqrt((segment[0][0]-triangle_intersions[j][1][0])**2+(segment[0][1]-triangle_intersions[j][1][1])**2) < tolerance + + test_P2_segment_P1_intersection = sqrt((segment[1][0]-triangle_intersions[j][0][0])**2+(segment[1][1]-triangle_intersions[j][0][1])**2) < tolerance + test_P2_segment_P2_intersection = sqrt((segment[1][0]-triangle_intersions[j][1][0])**2+(segment[1][1]-triangle_intersions[j][1][1])**2) < tolerance + + if test_P1_segment_P1_intersection != test_P2_segment_P2_intersection: + neighbor.append(triangle_intersions[j]) + neighbor_indices.append(j) + if test_P1_segment_P2_intersection != test_P2_segment_P1_intersection: + neighbor.append(triangle_intersions[j]) + neighbor_indices.append(j) + return (neighbor, neighbor_indices) + +class StlRepresentation(): + def __init__(self, filename): + self.filename = filename + self.__import_stl_file__() + + def __import_stl_file__(self): + stl_mesh = mesh.Mesh.from_file(self.filename) + self.vertices = stl_mesh.vectors + return 0 + + def __identify_sliced_vertices__(self,a,b,c,d): + intersections = set() + triangle_intersions = [] + for triangle in self.vertices: + triangle_intersion = [] + # Parcourir les 3 arêtes du triangle + for i in range(3): + p1 = triangle[i] + p2 = triangle[(i + 1) % 3] + # Calculer les valeurs signées de p1 et p2 par rapport au plan + val1 = a * p1[0] + b * p1[1] + c * p1[2] + d + val2 = a * p2[0] + b * p2[1] + c * p2[2] + d + # Si les signes sont opposés, il y a intersection + if val1 * val2 < 0: + intersection = intersection_segment_plan(p1, p2, a, b, c, d) + if intersection is not None: + intersections.add(tuple(intersection)) + triangle_intersion.append(tuple(intersection)) + if len(triangle_intersion) >0: + triangle_intersions.append(triangle_intersion) + return (intersections,triangle_intersions) + + def slice_stl_representation(self, a,b,c,d): + (intersections,triangle_intersions) = self.__identify_sliced_vertices__(a,b,c,d) + intersections = list(intersections) + intersections_x = [intersections[i][0] for i in range(0, len(intersections))] + intersections_y = [intersections[i][1] for i in range(0, len(intersections))] + + contours = {} + index = 1 + contours[index] = [triangle_intersions[0]] + (neighbor, neighbor_indices) = find_neighbor(triangle_intersions[0], triangle_intersions, 0.001) + + for i in range(0,len(neighbor)): + contours[index].append(neighbor[i]) + + treated_indices = [0] + test = False + index = 1 + + while test == False: + + indices_to_be_tested = neighbor_indices.copy() + neighbor_indices = [None]*len(indices_to_be_tested) + neighbor = [None]*len(indices_to_be_tested) + + for i in range(0,len(indices_to_be_tested)): + if indices_to_be_tested[i] not in treated_indices: + (neighbor[i], neighbor_indices[i]) = find_neighbor(triangle_intersions[indices_to_be_tested[i]], triangle_intersions, 0.001) + + treated_indices.append(indices_to_be_tested[i]) + new_indices = [] + for k in range(0,len(neighbor[i])): + if neighbor[i] != [None]: + contours[index].append(neighbor[i][k]) + new_indices.append(neighbor_indices[i][k]) + + indices_to_be_tested = new_indices.copy() + + + + plt.figure(figsize=(16, 12)) + plt.plot(intersections_x,intersections_y,'r*') + for i in range(0,len(contours[1])): + plt.plot([contours[1][i][0][0], contours[1][i][1][0]], + [contours[1][i][0][1], contours[1][i][1][1]], + 'b-') + plt.plot([triangle_intersions[0][0][0], triangle_intersions[0][1][0]], + [triangle_intersions[0][0][1], triangle_intersions[0][1][1]], + 'g-') + plt.axis("equal") + plt.show() + + + return 0 + + def plot_sliced_stl(self,a,b,c,d): + return 0 + + + def get_layer_internal_contour(self, stl_slice): + return 0 + + def get_layer_external_contour(self, stl_slice): + return 0 + +if __name__ == '__main__': + print("demarrage") + STL_representation = StlRepresentation("/home/mosserloic/Documents/example_cylinder.stl") + STL_representation.slice_stl_representation(0,0,1,-25) diff --git a/3D_printer/src/printer3d_pointCloud_processing/nodes/point_cloud_processing_node.py b/3D_printer/src/printer3d_pointCloud_processing/nodes/point_cloud_processing_node.py new file mode 100755 index 0000000..2173977 --- /dev/null +++ b/3D_printer/src/printer3d_pointCloud_processing/nodes/point_cloud_processing_node.py @@ -0,0 +1,385 @@ +#!/usr/bin/env python3 +# 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. + +import rclpy +from rclpy.node import Node +from printer3d_msgs.srv import GcodeCommand +import numpy as np +# import pyvista as pv +import matplotlib.pyplot as plt +import matplotlib as mpl +import random +import pyransac3d as pyrsc +from math import sqrt +from printer3d_gocator_msgs.srv import PTCloudTreat +from constants import * +from mask_creation_utility import * + +""" parametres du noeuds ROS, à mettre dans un fichier de config a la fin""" +basic_limit_for_reflexions = 12 + +coords = [] + +def onclick(event): + ix, iy = event.xdata, event.ydata + print ('x = ',ix,', y = ',iy) + global coords + coords.append((ix, iy)) + if len(coords) == 2: + print('ok') + plt.close() + return coords + +class PointCloudProcessingNode(Node): + def __init__(self): + """PointCloudProcessingNode class constructor.""" + super().__init__('point_cloud_processing_node') + self.xinf = -100000 + self.xsup = 100000 + self.x1 = -100000 + self.x2 = 100000 + self.y1 = -100000 + self.y2 = 100000 + self.pointCloudManipService = self.create_service(PTCloudTreat, 'manip_PTCloud', self.manip_point_cloud) + + def manip_point_cloud(self, request, response): + + order = request.order + filename = request.filenamePointCloud + + try: + point_Cloud = self.load_scan(filename) + except: + self.get_logger.info("non existing point cloud : "+filename) + exit() + + if order == ORDRE_INITIALISATION: + + reference_layer_filtered = self.remove_non_used_plate_points(point_Cloud) + self.transformation_creation(reference_layer_filtered) + + elif order == ORDRE_CALC_M_S: + + filtered_scan = self.remove_non_used_plate_points(point_Cloud) + transformed_filtered_scan = self.tranform_point_cloud(filtered_scan) + + else: + self.get_logger.info("non existing order number : "+filename) + exit() + + return 0 + + def load_scan(self,fileName): + data = np.squeeze(np.load(fileName)) + xDim = data.shape[1] + yDim = data.shape[0] + zDim = data.shape[2] + for i in range(0,yDim): + for j in range(0,xDim): + data[i,j,1] = i*0.3 + return data + + def flatten_point_cloud(self,inputPointCloud): + if len(inputPointCloud.shape) == 2: + outputPointCloud = inputPointCloud + else: + outputPointCloud = np.reshape(inputPointCloud,(inputPointCloud.shape[0]*inputPointCloud.shape[1],inputPointCloud.shape[2])) + return outputPointCloud + + def remove_non_used_plate_points(self,inputPointCloud): + treatedPointCloud = self.remove_inf_value(inputPointCloud) + shapes = treatedPointCloud.shape + savedPoints = [] + for i in range(0,shapes[0]): + if treatedPointCloud[i,0]>self.xinf and treatedPointCloud[i,0]self.x1 and treatedPointCloud[i,0]self.y1 and treatedPointCloud[i,1]zMoy-epsilon and treatedPointCloud[i,2] xDomain[0] \ + and treatedPointCloud[i,0] < xDomain[1] \ + and treatedPointCloud[i,1] > yDomain[0] \ + and treatedPointCloud[i,1] < yDomain[1]: + savedPoints.append(treatedPointCloud[i,:]) + savedPoints = np.array(savedPoints) + return savedPoints + + def get_Mj(self,layerPointCloud, layerNumber, layerHeight): + shapes = layerPointCloud.shape + Mj = 0 + for i in range(0,shapes[0]): + Mj += layerPointCloud[i,2] + return (Mj/shapes[0])-(layerHeight*layerNumber) + + def get_Sj(self,layerPointCloud,Mj): + shapes = layerPointCloud.shape + Sj = 0 + for i in range(0,shapes[0]): + Sj += (layerPointCloud[i,2]-Mj)**2 + Sj /= shapes[0] + return sqrt(Sj) + + def remove_inf_value(self,inputPointCloud): + inputPointCloud = self.flatten_point_cloud(inputPointCloud) + shapes = inputPointCloud.shape + outputPointCloud = [] + for i in range(0,shapes[0]): + if inputPointCloud[i,2]<10000 and inputPointCloud[i,2]>-10000: + outputPointCloud.append(inputPointCloud[i,:]) + return np.array(outputPointCloud) + + def remove_value_above(self,inputPointCloud,threshold): + inputPointCloud = self.flatten_point_cloud(inputPointCloud) + shapes = inputPointCloud.shape + outputPointCloud = [] + for i in range(0,shapes[0]): + if inputPointCloud[i,2] 0: + best_eq[3] = -best_eq[3] + + if best_eq[2]<0: + self.rotation_matrix[2,0] = -best_eq[0] + self.rotation_matrix[2,1] = -best_eq[1] + self.rotation_matrix[2,2] = -best_eq[2] + self.translation_matrix[2] = best_eq[3] + self.zVector = best_eq[0:3] + self.zVector[0] = -self.zVector[0] + self.zVector[1] = -self.zVector[1] + self.zVector[2] = -self.zVector[2] + else: + self.rotation_matrix[2,0] = best_eq[0] + self.rotation_matrix[2,1] = best_eq[1] + self.rotation_matrix[2,2] = best_eq[2] + self.translation_matrix[2] = best_eq[3] + self.zVector = best_eq[0:3] + + self.xVector = plate_point_cloud[10,:]-plate_point_cloud[0,:] + self.xVector /= np.sqrt(self.xVector[0]**2+self.xVector[1]**2+self.xVector[2]**2) + + self.yVector = [0, 0, 0] + + self.yVector[0] = self.zVector[1]*self.xVector[2] - self.zVector[2]*self.xVector[1] + self.yVector[1] = self.zVector[2]*self.xVector[0] - self.zVector[0]*self.xVector[2] + self.yVector[2] = self.zVector[0]*self.xVector[1] - self.zVector[1]*self.xVector[0] + + self.rotation_matrix[0,0] = self.xVector[0] + self.rotation_matrix[0,1] = self.xVector[1] + self.rotation_matrix[0,2] = self.xVector[2] + + self.rotation_matrix[1,0] = self.yVector[0] + self.rotation_matrix[1,1] = self.yVector[1] + self.rotation_matrix[1,2] = self.yVector[2] + + return 0 + + def tranform_point_cloud(self,inputPointCloud): + outputPointCloud = np.zeros(inputPointCloud.shape) + inputShape = inputPointCloud.shape + for i in range(0,inputShape[0]): + outputPointCloud[i,:] = np.dot(self.rotation_matrix, inputPointCloud[i,:]) - self.translation_matrix + return outputPointCloud + + def decimate_point_cloud(self,inputPointCloud,decimationRate): + shapes = inputPointCloud.shape + interPointCloud = self.flatten_point_cloud(inputPointCloud) + indexList = [i for i in range(0,interPointCloud.shape[0])] + deletedElementsIndex = random.sample(indexList,int(len(indexList)*decimationRate)) + outputPointCloud = np.delete(interPointCloud, deletedElementsIndex,axis=0) + return outputPointCloud + + def ask_for_plate_limits(self,inputPointCloud): + data = self.remove_inf_value(inputPointCloud) + data = self.decimate_point_cloud(data, 0.95) + x = data[:,0] + y = data[:,1] + z = data[:,2] + fig = plt.figure(figsize=(8, 8)) + ax = fig.add_subplot(111,aspect='equal') + im = ax.scatter(x, y, c=z,s=1) + plt.xlabel('x (mm)') + plt.ylabel('y (mm)') + fig.colorbar(im) + ax.set_xlim(min(x)-10, max(x)+10) + ax.set_ylim(min(y)-10, max(y)+10) + cid = fig.canvas.mpl_connect('button_press_event', onclick) + plt.show() + global coords + self.xinf = coords[0][0] + self.xsup = coords[1][0] + self.get_logger().info("xinf : "+str(self.xinf)) + self.get_logger().info("xsup : "+str(self.xsup)) + coords = [] + return 0 + + def ask_for_points_of_plate(self,inputPointCloud): + data = self.remove_inf_value(inputPointCloud) + data = self.decimate_point_cloud(data, 0.95) + x = data[:,0] + y = data[:,1] + z = data[:,2] + fig = plt.figure(figsize=(8, 8)) + ax = fig.add_subplot(111,aspect='equal') + im = ax.scatter(x, y, c=z,s=1) + plt.xlabel('x (mm)') + plt.ylabel('y (mm)') + fig.colorbar(im) + ax.set_xlim(min(x)-10, max(x)+10) + ax.set_ylim(min(y)-10, max(y)+10) + cid = fig.canvas.mpl_connect('button_press_event', onclick) + plt.show() + global coords + self.x1 = coords[0][0] + self.x2 = coords[1][0] + self.y1 = coords[0][1] + self.y2 = coords[1][1] + self.get_logger().info("x1 : "+str(self.x1)) + self.get_logger().info("x2 : "+str(self.x2)) + self.get_logger().info("y1 : "+str(self.y1)) + self.get_logger().info("y2 : "+str(self.y2)) + coords = [] + return 0 + + def extract_reference_base_change(self): + return 0 + +if __name__ == '__main__': + rclpy.init() + point_cloud_processing_node = PointCloudProcessingNode() + + base_filename = "/home/gulltor/Documents/history/impression_base_avec_retraction/scan" + reference_layer = point_cloud_processing_node.load_scan(base_filename+"/layer_scan_0.npy") + layer_3 = point_cloud_processing_node.load_scan(base_filename+"/layer_scan_3.npy") + layer_3 = point_cloud_processing_node.remove_value_above(layer_3, -70) + + #point_cloud_processing_node.ask_for_plate_limits(layer_3) + point_cloud_processing_node.xinf = -40.311554355006095 + point_cloud_processing_node.xsup = 4 + + filtered_scan = point_cloud_processing_node.remove_non_used_plate_points(layer_3) + + reference_layer_filtered = point_cloud_processing_node.remove_non_used_plate_points(reference_layer) + + point_cloud_processing_node.transformation_creation(reference_layer_filtered) + + layer_3_transformed = point_cloud_processing_node.tranform_point_cloud(filtered_scan) + + point_cloud_processing_node.plot_point_cloud(layer_3_transformed) + + point_cloud_processing_node.ask_for_points_of_plate(filtered_scan) + [xg, yg] = point_cloud_processing_node.find_layer_barycentre(filtered_scan, 0.04) + print("( ",xg," , ",yg," )") + + + point_cloud_list = [None]*122 + Mj = [None]*122 + Sj = [None]*122 + + for i in range(0,122): + point_cloud_list[i] = point_cloud_processing_node.load_scan(base_filename+'/layer_scan_'+str(i)+'.npy') + point_cloud_list[i] = point_cloud_processing_node.remove_non_used_plate_points(point_cloud_list[i]) + point_cloud_list[i] = point_cloud_processing_node.select_points_of_layer(point_cloud_list[i], [xg-7, xg+7], [yg-7, yg+7]) + Mj[i] = point_cloud_processing_node.get_Mj(point_cloud_list[i], i, 0.41) + Sj[i] = point_cloud_processing_node.get_Sj(point_cloud_list[i],Mj[i]) + point_cloud_processing_node.get_logger().info('layer : '+str(i)) + + plt.figure() + plt.plot(Mj) + plt.show() + + plt.figure() + plt.plot(Sj) + plt.show() + + + rclpy.shutdown() diff --git a/3D_printer/src/printer3d_pointCloud_processing/package.xml b/3D_printer/src/printer3d_pointCloud_processing/package.xml new file mode 100755 index 0000000..5d20ba4 --- /dev/null +++ b/3D_printer/src/printer3d_pointCloud_processing/package.xml @@ -0,0 +1,18 @@ + + + + printer3d_point_cloud_processing + 0.0.0 + TODO: Package description + gulltor + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/3D_printer/src/printer3d_profile_capture/CMakeLists.txt b/3D_printer/src/printer3d_profile_capture/CMakeLists.txt new file mode 100644 index 0000000..22e05ea --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/CMakeLists.txt @@ -0,0 +1,82 @@ +cmake_minimum_required(VERSION 3.10) +project(printer3d_profile_capture) + +# Standards C/C++ +set(CMAKE_C_STANDARD 99) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Dépendances (doit venir avant add_executable) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(printer3d_gocator_msgs REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL REQUIRED COMPONENTS common) +find_package(Boost REQUIRED COMPONENTS system) + +# Chemins GO_SDK (avant add_executable) +set(GO_SDK_4 ${CMAKE_CURRENT_SOURCE_DIR}/external/GO_SDK) +find_path(GOCATOR_INCLUDES NAMES GoSdk/GoSdk.h PATHS ${GO_SDK_4}/Gocator/GoSdk) +find_path(KAPI_INCLUDES NAMES kApi/kApi.h PATHS ${GO_SDK_4}/Platform/kApi) +find_library(GOCATOR_LIBRARIES NAMES GoSdk PATHS ${GO_SDK_4}/lib/linux_x64d) +find_library(KAPI_LIBRARIES NAMES kApi PATHS ${GO_SDK_4}/lib/linux_x64d) + +# Sources +set(LIB_SRCS src/driver/gocatorSensor.cpp) +set(LIB_HDRS src/driver/gocatorSensor.h) + +# --- Création de la cible --- +add_executable(gocator_sensor_node + src/nodes/gocatorSensorNode.cpp + ${LIB_SRCS} +) + +# --- Configuration de la cible (APRES add_executable) --- +target_include_directories(gocator_sensor_node PRIVATE + ${GOCATOR_INCLUDES} + ${KAPI_INCLUDES} + ${PCL_INCLUDE_DIRS} +) + +target_link_directories(gocator_sensor_node PRIVATE + ${PCL_LIBRARY_DIRS} + ${GO_SDK_4}/lib/linux_x64d +) + +target_link_libraries(gocator_sensor_node + ${GOCATOR_LIBRARIES} + ${KAPI_LIBRARIES} + ${Boost_SYSTEM_LIBRARY} +) + +ament_target_dependencies(gocator_sensor_node + rclcpp + sensor_msgs + std_msgs + printer3d_gocator_msgs + pcl_conversions + Boost +) + +# Installation +install(TARGETS gocator_sensor_node DESTINATION lib/${PROJECT_NAME}) +install( + DIRECTORY external/GO_SDK/lib/linux_x64d/ + DESTINATION lib/${PROJECT_NAME}/ +) + +# Tests +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() \ No newline at end of file diff --git a/3D_printer/src/printer3d_profile_capture/package.xml b/3D_printer/src/printer3d_profile_capture/package.xml new file mode 100644 index 0000000..1087359 --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/package.xml @@ -0,0 +1,22 @@ + + +printer3d_profile_capture +0.0.0 +Messages form communication with GOCATOR systems +m.bednarczyk +TODO: License declaration +ament_cmake +ament_lint_auto +ament_lint_common +std_msgs +sensor_msgs +printer3d_gocator_msgs + +builtin_interfaces +pcl_conversions +rosidl_default_generators +rosidl_interface_packages + +ament_cmake + + diff --git a/3D_printer/src/printer3d_profile_capture/src/driver/CMakeLists.txt b/3D_printer/src/printer3d_profile_capture/src/driver/CMakeLists.txt new file mode 100644 index 0000000..d3dae06 --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/src/driver/CMakeLists.txt @@ -0,0 +1,62 @@ +# Pre-requisites about cmake itself +cmake_minimum_required(VERSION 3.5) + +# Project name and the type of project +project(gocatorSensor) + +# DEBUG/RELEASE +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "RELEASE") +endif() + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# 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 point cloud Library +find_package(PCL REQUIRED COMPONENTS) +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +# Set GO_SDK include paths (That could be part of a FindGocator.cmake) +set(GO_SDK_4 ${CMAKE_CURRENT_SOURCE_DIR}/../../external/GO_SDK) +find_path( + GOCATOR_INCLUDES + NAMES GoSdk/GoSdk.h + PATHS ${GO_SDK_4}/Gocator/GoSdk) +find_path( + KAPI_INCLUDES + NAMES kApi/kApi.h + PATHS ${GO_SDK_4}/Platform/kApi) +include_directories(${GOCATOR_INCLUDES} ${KAPI_INCLUDES}) + +# Set GO_SDK libs (That could be part of a FindGocator.cmake) +find_library( + GOCATOR_LIBRARIES + NAMES GoSdk + PATHS ${GO_SDK_4}/lib/linux_x64d/) +find_library( + KAPI_LIBRARIES + NAMES kApi + PATHS ${GO_SDK_4}/lib/linux_x64d/) + +# Set source files +set(SRCS gocatorSensor.cpp) + +# Set header files +set(HDRS gocatorSensor.h) + +#Build library +add_library(${PROJECT_NAME} SHARED ${SRCS}) +target_link_libraries(${PROJECT_NAME} ${GOCATOR_LIBRARIES} ${KAPI_LIBRARIES} ${PCL_COMMON_LIBRARIES} ${PCL_IO_LIBRARIES}) diff --git a/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.cpp b/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.cpp new file mode 100644 index 0000000..3877eeb --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.cpp @@ -0,0 +1,421 @@ +// 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. + +#include "gocatorSensor.h" + +gocator_sensor::Device::Device(const std::string & _ip_address) +{ + kStatus status; + kIpAddress ipAddress; + kChar model_name[50]; + + // init all GO API objects + go_api_ = kNULL; + go_system_ = kNULL; + go_sensor_ = kNULL; + go_setup_ = kNULL; + go_dataset_ = kNULL; + go_stamp_ptr_ = kNULL; + + // construct Gocator API Library + if ((status = GoSdk_Construct(&go_api_)) != kOK) { + std::cout << "Device(). Error: GoSdk_Construct: " << status << std::endl; + status_ = DEVICE_NOT_FOUND; + return; + } + + // construct GoSystem object + if ((status = GoSystem_Construct(&go_system_, kNULL)) != kOK) { + std::cout << "Device(). Error: GoSystem_Construct: " << status << std::endl; + status_ = DEVICE_NOT_FOUND; + return; + } + + // obtain GoSensor object by sensor IP address + kIpAddress_Parse(&ipAddress, _ip_address.c_str()); + if ((status = GoSystem_FindSensorByIpAddress(go_system_, &ipAddress, &go_sensor_)) != kOK) { + std::cout << "Device(). Error: GoSystem_FindSensorByIpAddress: " << status << std::endl; + status_ = DEVICE_NOT_FOUND; + return; + } + + // Success case. Set status and device fixed params (ip, model name and serial number ). + status_ = DEVICE_FOUND; + device_params_.ip_address_ = _ip_address; + + // create connection to GoSensor object + if ((status = GoSensor_Connect(go_sensor_)) != kOK) { + std::cout << "Device(). Error: GoSensor_Connect: " << status << std::endl; + status_ = DEVICE_NOT_CONNECT; + return; + } + status_ = DEVICE_CONNECT; + + // enable sensor data channel + if ((status = GoSystem_EnableData(go_system_, kTRUE)) != kOK) { + std::cout << "Device(). Error: GoSensor_EnableData: " << status << std::endl; + return; + } + + // retrieve setup handle + if ((go_setup_ = GoSensor_Setup(go_sensor_)) == kNULL) { + std::cout << "Device(). Error: GoSensor_Setup: Invalid Handle" << std::endl; + return; + } + + // Obtain camera model + if ((status = GoSensor_Model(go_sensor_, model_name, 50)) != kOK) { + std::cout << "Device(). Error: GoSensor_Model: " << status << std::endl; + return; + } + device_params_.model_name_ = model_name; + + // Obtain camera Serial number + device_params_.sn_ = (unsigned int)GoSensor_Id(go_sensor_); + + // Obtain exposure + capture_params_.exposure_time_ = GoSetup_Exposure(go_setup_, GO_ROLE_MAIN); + + // Obtain spacing interval + capture_params_.spacing_interval_ = GoSetup_SpacingInterval(go_setup_, GO_ROLE_MAIN); + + // print info + std::cout << "Found Sensor: " << std::endl; + device_params_.print(); +} + +gocator_sensor::Device::~Device() +{ + kStatus status; + + // destroy handles + GoDestroy(go_system_); + GoDestroy(go_api_); + + // bye bye message + std::cout << "~Device(). Gocator Sensor Stopped and Device Object Destroyed." << std::endl; +} + +int gocator_sensor::Device::configure(const CaptureParams & _configs) +{ + kStatus status; + + // set exposure + if ((status = GoSetup_SetExposure(go_setup_, GO_ROLE_MAIN, _configs.exposure_time_)) != kOK) { + std::cout << "configure(): Error setting Exposure Time to " << _configs.exposure_time_ << + std::endl; + return -1; + } + + // set spacing interval + if ((status = + GoSetup_SetSpacingInterval(go_setup_, GO_ROLE_MAIN, _configs.spacing_interval_)) != kOK) + { + std::cout << "configure(): Error setting Spacing Interval to " << _configs.spacing_interval_ << + std::endl; + return -1; + } + + // set this->capture_params_ with true values from camera + capture_params_.exposure_time_ = GoSetup_Exposure(go_setup_, GO_ROLE_MAIN); + capture_params_.spacing_interval_ = GoSetup_SpacingInterval(go_setup_, GO_ROLE_MAIN); + + // print + std::cout << "Configuration Settings: " << std::endl; + capture_params_.print(); + + // return + return 1; +} + +int gocator_sensor::Device::start() +{ + kStatus status; + + // start Gocator sensor + if ((status = GoSystem_Start(go_system_)) != kOK) { + std::cout << "Device(). Error: GoSystem_Start: " << status << std::endl; + return -1; + } + + // message to std out + // std::cout << "Gocator running ... " << std::endl; + + // set this->status_ + this->status_ = DEVICE_RUNNING; + + // return success + return 1; +} + +int gocator_sensor::Device::stop() +{ + kStatus status; + + // stop Gocator sensor + if ((status = GoSystem_Stop(go_system_)) != kOK) { + std::cout << "~Device(). Error: GoSystem_Stop: " << status << std::endl; + return -1; + } + + // message to std out + // std::cout << "... Gocator stopped" << std::endl << std::endl; + + // set this->status_ + this->status_ = DEVICE_CONNECT; + + // return success + return 1; +} + +int gocator_sensor::Device::getCurrentSnapshot(pcl::PointCloud & _p_cloud) +{ +} + +int gocator_sensor::Device::getSingleSnapshot(pcl::PointCloud & _p_cloud) +{ +} + +int gocator_sensor::Device::getProfile(pcl::PointCloud & _p_cloud) +{ + unsigned int i, j, k, arrayIndex; + GoDataSet dataset = kNULL; + std::vector profileBuffer; + GoStamp * stamp = kNULL; + GoDataMsg dataObj; + k32u profilePointCount; + if (GoSystem_ReceiveData(go_system_, &dataset, RECEIVE_TIMEOUT) == kOK) { + printf("Data message received:\n"); + printf("Dataset count: %u\n", (k32u)GoDataSet_Count(dataset)); + // each result can have multiple data items + // loop through all items in result message + for (i = 0; i < GoDataSet_Count(dataset); ++i) { + dataObj = GoDataSet_At(dataset, i); + // Retrieve GoStamp message + switch (GoDataMsg_Type(dataObj)) { + case GO_DATA_MESSAGE_TYPE_STAMP: + { + GoStampMsg stampMsg = dataObj; + + printf("Stamp Message batch count: %u\n", (k32u)GoStampMsg_Count(stampMsg)); + for (j = 0; j < GoStampMsg_Count(stampMsg); ++j) { + stamp = GoStampMsg_At(stampMsg, j); + printf(" Timestamp: %llu\n", stamp->timestamp); + printf(" Encoder: %lld\n", stamp->encoder); + printf(" Frame index: %llu\n", stamp->frameIndex); + } + } + break; + case GO_DATA_MESSAGE_TYPE_UNIFORM_PROFILE: + { + GoResampledProfileMsg profileMsg = dataObj; + + printf( + "Resampled Profile Message batch count: %u\n", + (k32u)GoResampledProfileMsg_Count(profileMsg)); + + _p_cloud.height = GoResampledProfileMsg_Count(profileMsg); + _p_cloud.width = GoResampledProfileMsg_Width(profileMsg); + _p_cloud.resize(_p_cloud.height * _p_cloud.width); + + for (k = 0; k < GoResampledProfileMsg_Count(profileMsg); ++k) { + unsigned int validPointCount = 0; + short * data = GoResampledProfileMsg_At(profileMsg, k); + double XResolution = NM_TO_MM(GoResampledProfileMsg_XResolution(profileMsg)); + double ZResolution = NM_TO_MM(GoResampledProfileMsg_ZResolution(profileMsg)); + double XOffset = UM_TO_MM(GoResampledProfileMsg_XOffset(profileMsg)); + double ZOffset = UM_TO_MM(GoResampledProfileMsg_ZOffset(profileMsg)); + + // profileBuffer.resize(GoResampledProfileMsg_Width(profileMsg)); + + + // translate 16-bit range data to engineering units and copy profiles to memory array + for (arrayIndex = 0; arrayIndex < GoResampledProfileMsg_Width(profileMsg); + ++arrayIndex) + { + if (data[arrayIndex] != INVALID_RANGE_16BIT) { + // profileBuffer[arrayIndex].x = XOffset + XResolution * arrayIndex; + // profileBuffer[arrayIndex].z = ZOffset + ZResolution * data[arrayIndex]; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).x = XOffset + XResolution * + arrayIndex; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).z = ZOffset + ZResolution * + data[arrayIndex]; + validPointCount++; + } else { + // profileBuffer[arrayIndex].x = XOffset + XResolution * arrayIndex; + // profileBuffer[arrayIndex].z = INVALID_RANGE_DOUBLE; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).x = XOffset + XResolution * + arrayIndex; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).z = INVALID_RANGE_DOUBLE; + } + } + printf( + " Profile Valid Point %d out of max %d\n", validPointCount, + profilePointCount); + } + } + break; + case GO_DATA_MESSAGE_TYPE_PROFILE_POINT_CLOUD: // Note this is NON resampled profile + { + GoProfileMsg profileMsg = dataObj; + + printf("Profile Message batch count: %u\n", (k32u)GoProfileMsg_Count(profileMsg)); + + _p_cloud.height = GoProfileMsg_Count(profileMsg); + _p_cloud.width = GoProfileMsg_Width(profileMsg); + _p_cloud.resize(_p_cloud.height * _p_cloud.width); + + for (k = 0; k < GoProfileMsg_Count(profileMsg); ++k) { + kPoint16s * data = GoProfileMsg_At(profileMsg, k); + unsigned int validPointCount = 0; + double XResolution = NM_TO_MM(GoProfileMsg_XResolution(profileMsg)); + double ZResolution = NM_TO_MM(GoProfileMsg_ZResolution(profileMsg)); + double XOffset = UM_TO_MM(GoProfileMsg_XOffset(profileMsg)); + double ZOffset = UM_TO_MM(GoProfileMsg_ZOffset(profileMsg)); + + // profileBuffer.resize(GoResampledProfileMsg_Width(profileMsg)); + + // translate 16-bit range data to engineering units and copy profiles to memory array + for (arrayIndex = 0; arrayIndex < GoProfileMsg_Width(profileMsg); ++arrayIndex) { + if (data[arrayIndex].x != INVALID_RANGE_16BIT) { + // profileBuffer[arrayIndex].x = XOffset + XResolution * data[arrayIndex].x; + // profileBuffer[arrayIndex].z = ZOffset + ZResolution * data[arrayIndex].y; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).x = XOffset + XResolution * + data[arrayIndex].x; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).z = ZOffset + ZResolution * + data[arrayIndex].y; + validPointCount++; + } else { + // profileBuffer[arrayIndex].x = INVALID_RANGE_DOUBLE; + // profileBuffer[arrayIndex].z = INVALID_RANGE_DOUBLE; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).x = INVALID_RANGE_DOUBLE; + _p_cloud.points.at(k * _p_cloud.width + arrayIndex).z = INVALID_RANGE_DOUBLE; + } + } + printf( + " Profile Valid Point %d out of max %d\n", validPointCount, + profilePointCount); + } + } + break; + case GO_DATA_MESSAGE_TYPE_PROFILE_INTENSITY: + { + unsigned int validPointCount = 0; + GoProfileIntensityMsg intensityMsg = dataObj; + printf( + "Intensity Message batch count: %u\n", + (k32u)GoProfileIntensityMsg_Count(intensityMsg)); + + profileBuffer.resize(GoProfileIntensityMsg_Count(intensityMsg)); + + for (k = 0; k < GoProfileIntensityMsg_Count(intensityMsg); ++k) { + unsigned char * data = GoProfileIntensityMsg_At(intensityMsg, k); + for (arrayIndex = 0; arrayIndex < GoProfileIntensityMsg_Width(intensityMsg); + ++arrayIndex) + { + profileBuffer[arrayIndex].intensity = data[arrayIndex]; + } + } + } + break; + } + } + GoDestroy(dataset); + } else { + printf("Error: No data received during the waiting period\n"); + } + return 1; +} + +void gocator_sensor::Device::getDeviceHealth(std::string & _health_str) const +{ + // local variables + GoDataSet health_data = kNULL; + GoHealthMsg health_msg = kNULL; + GoIndicator * health_indicator = kNULL; + std::ostringstream sstr; + + // get health from device + if ( (GoSystem_ReceiveHealth(go_system_, &health_data, RECEIVE_TIMEOUT)) == kOK) { + for (unsigned int ii = 0; ii < GoDataSet_Count(health_data); ii++) { + health_msg = GoDataSet_At(health_data, ii); + for (unsigned int jj = 0; jj < GoHealthMsg_Count(health_msg); jj++) { + health_indicator = GoHealthMsg_At(health_msg, jj); + sstr << "Indicator[" << jj << "]:\n" + << "\tId: " << health_indicator->id << "\n" + << "\tInstance: " << health_indicator->instance << "\n" + << "\tValue: " << health_indicator->value << "\n"; + } + } + GoDestroy(health_msg); + } + + _health_str = sstr.str(); +} + +void gocator_sensor::Device::getTemperature( + double & _internal_temp, double & _projector_temp, + double & _laser_temp) const +{ + // local variables + GoDataSet health_data = kNULL; + GoHealthMsg health_msg = kNULL; + GoIndicator * health_indicator = kNULL; + // k32u instance; + + // get health dataset from device + if ( (GoSystem_ReceiveHealth(go_system_, &health_data, RECEIVE_TIMEOUT)) == kOK) { + for (unsigned int ii = 0; ii < GoDataSet_Count(health_data); ii++) { + // get the health message + health_msg = GoDataSet_At(health_data, ii); + + // find in the message the internal temperature indicator, and set the value + health_indicator = GoHealthMsg_Find(health_msg, GO_HEALTH_TEMPERATURE, 0); + if (health_indicator != kNULL) {_internal_temp = health_indicator->value;} else { + _internal_temp = -100.; + } + + // find in the message the projector temperature indicator, and set the value + health_indicator = GoHealthMsg_Find(health_msg, GO_HEALTH_PROJECTOR_TEMPERATURE, 0); + if (health_indicator != kNULL) {_projector_temp = health_indicator->value;} else { + _projector_temp = -100.; + } + + // find in the message the projector temperature indicator, and set the value + health_indicator = GoHealthMsg_Find(health_msg, GO_HEALTH_LASER_TEMPERATURE, 0); + if (health_indicator != kNULL) {_laser_temp = health_indicator->value;} else { + _laser_temp = -100.; + } + } + GoDestroy(health_msg); + } +} + +int gocator_sensor::Device::close() +{ +} + +void gocator_sensor::Device::printDeviceData() const +{ +} + +void gocator_sensor::Device::sendTrigger() const +{ + printf("sending trigger \n"); + kStatus status = GoSensor_Trigger(go_sensor_); + if (status != kOK) { + printf("Error: GoSensor_Connect:%d\n", status); + return; + } +} diff --git a/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.h b/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.h new file mode 100644 index 0000000..1421df0 --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/src/driver/gocatorSensor.h @@ -0,0 +1,205 @@ +// 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. + +#ifndef DRIVER__GOCATORSENSOR_H_ +#define DRIVER__GOCATORSENSOR_H_ + +// GoSdk +#include +// PCL +#include +#include + +// std c/c++ +#include +#include +#include +#include +#include + +// constants +#define SENSOR_IP "192.168.1.10" // sensor IP address +#define RECEIVE_TIMEOUT 20000000 // timeout for snapshot acquisition +#define INVALID_RANGE_16BIT ((short)0x8000) // gocator transmits range data +// as 16-bit signed integers. 0x8000 signifies invalid range data. +#define DOUBLE_MAX ((k64f)1.7976931348623157e+308) // 64-bit double - largest positive value. +#define INVALID_RANGE_DOUBLE ((k64f) - DOUBLE_MAX) // floating point value to +// represent invalid range data. +#define NM_TO_MM(VALUE) (((k64f)(VALUE)) / 1000000.0) +#define UM_TO_MM(VALUE) (((k64f)(VALUE)) / 1000.0) + +namespace gocator_sensor +{ + + typedef struct ProfilePoint + { + double x; // x-coordinate in engineering units (mm) - position along laser line + double z; // z-coordinate in engineering units (mm) - height (at the given x position) + unsigned char intensity; + } ProfilePoint; + +// status values + enum {DEVICE_NOT_FOUND = 0, DEVICE_FOUND, DEVICE_NOT_CONNECT, DEVICE_CONNECT, DEVICE_RUNNING}; + +// device parameters struct + struct DeviceParams + { + std::string ip_address_; + std::string model_name_; + unsigned int sn_; + + void print() const + { + std::cout << "\tIP ad: \t" << ip_address_ << std::endl; + std::cout << "\tModel: \t" << model_name_ << std::endl; + std::cout << "\tSN: \t" << sn_ << std::endl; + } + }; + +// device configuration struct + struct CaptureParams + { + double exposure_time_; // in useconds + double spacing_interval_; // in millimeters + void print() const + { + std::cout << "\texposure [us]: \t" << exposure_time_ << std::endl; + std::cout << "\tspacing [mm]: \t" << spacing_interval_ << std::endl; + } + }; + +// Device class + class Device + { +protected: + // current point cloud. Maybe not necessary to be here! + // pcl::PointCloud::Ptr p_cloud_; + // p_cloud_ = new pcl::PointCloud + + // driver status + unsigned int status_; + + // device fixed params + DeviceParams device_params_; + + // device configuration + CaptureParams capture_params_; + + // GO API objects + kAssembly go_api_; + GoSystem go_system_; + GoSensor go_sensor_; + GoSetup go_setup_; + GoDataSet go_dataset_; + GoStamp * go_stamp_ptr_; + +public: + /** \brief Constructor + * + * Constructor. Sets params_ struct. + * + **/ + explicit Device(const std::string & _ip_address); + + /** \brief Destructor + * + * Destructor + * + **/ + ~Device(); + + /** \brief Connect to a physical device given an ip address + * + * Connect to a physical device given an ip address + * + **/ + // int connect(); + + /** \brief Set/get device parameters to/from the camera + * + * Set/get device parameters to/from the camera + * + **/ + int configure(const CaptureParams & _configs); + + /** \brief Start device data acquisition + * + * Start device data acquisition + * + **/ + int start(); + + /** \brief Stop device dat acquisition + * + * Stop device data acquisition + * + **/ + int stop(); + + /** \brief Get the current snapshot + * + * Get the current snapshot, when in continuous acquisition + * + **/ + int getCurrentSnapshot(pcl::PointCloud < pcl::PointXYZ > & _p_cloud); + + /** \brief Get a single snapshot in the same thread + * + * Get a single snapshot in the same thread + * + **/ + int getSingleSnapshot(pcl::PointCloud < pcl::PointXYZ > & _p_cloud); + + int getProfile(pcl::PointCloud < pcl::PointXYZ > & _p_cloud); + + + /** \brief Returns all health data as a string + * + * Returns all health data as a string + * + **/ + void getDeviceHealth(std::string & _health_str) const; + + /** \brief Returns three device temperatures + * + * Returns three device temperatures: internal, projector and laser + * + **/ + void getTemperature( + double & _internal_temp, double & _projector_temp, + double & _laser_temp) const; + + /** \brief Close the connection to a physical device + * + * Close the connection to a physical device + * + **/ + int close(); + + /** \brief Print the device configuration + * + * Print the device configuration + * + **/ + void printDeviceData() const; + + /** \brief Send a trigger signal + * + **/ + void sendTrigger() const; + }; + +} // namespace gocator_sensor + +#endif // DRIVER__GOCATORSENSOR_H_ diff --git a/3D_printer/src/printer3d_profile_capture/src/nodes/gocatorSensorNode.cpp b/3D_printer/src/printer3d_profile_capture/src/nodes/gocatorSensorNode.cpp new file mode 100644 index 0000000..3b4c1f8 --- /dev/null +++ b/3D_printer/src/printer3d_profile_capture/src/nodes/gocatorSensorNode.cpp @@ -0,0 +1,82 @@ +// 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. + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "pcl_conversions/pcl_conversions.h" +#include "std_msgs/msg/empty.hpp" +#include "printer3d_gocator_msgs/srv/gocator_pt_cloud.hpp" +#include "../driver/gocatorSensor.h" + + +using namespace std::chrono_literals; + +class GocatorSensorNode : public rclcpp::Node +{ +public: + GocatorSensorNode() + : Node("gocatorSensorNode") + { + gsensor_ = new gocator_sensor::Device(SENSOR_IP); + capture_params_.exposure_time_ = 110; + capture_params_.spacing_interval_ = 0.1; + gsensor_->configure(capture_params_); + + service_ = this->create_service( + "gocator_get_profile", + std::bind( + &GocatorSensorNode::profile_snapshot, this, std::placeholders::_1, + std::placeholders::_2)); + // publisher_ = this->create_publisher("gocatorPTCloud_topic", 10); + } + +private: + void profile_snapshot( + std::shared_ptr request, + std::shared_ptr response) + { + pcl::PointCloud cloud; + // RCLCPP_INFO(this->get_logger(), "Processing service request!"); + gsensor_->start(); + gsensor_->sendTrigger(); + if (gsensor_->getProfile(cloud) == 1) { + auto pcloud = sensor_msgs::msg::PointCloud2(); + pcl::toROSMsg(cloud, pcloud); + pcloud.header.frame_id = "gocator_sensor"; + pcloud.header.stamp = this->get_clock()->now(); + + // publisher_->publish(pcloud); + + response->pcloud = pcloud; + } + } + + rclcpp::Service::SharedPtr service_; + rclcpp::Publisher::SharedPtr publisher_; + gocator_sensor::Device * gsensor_; + gocator_sensor::CaptureParams capture_params_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/README.md b/README.md index beee04b..a641071 100644 --- a/README.md +++ b/README.md @@ -1,2 +1,64 @@ # RAMSAI Github repository for the ANR RAMSAI project. + +# ROS2 keyence communication interface +## printer3d_keyence_profile_capture +This interface have been developped with ROS2 to propose a standard interface with Keyence LJ-X profilometer. It has been coded using Python language and integrates the python software delivered by Keyence to manage the communication with the profilometer. + +The software developped by Keyence have been initialy coded using C++. The python library implements a wrapper of this C++ library and load dynamic library (libljxacom.so in linux) and (LJX8_IF.dll in windows) + +Therefore, to have this package working, you need to get this external library and to put in the **./src/printer3d_keyence_profile_capture/src/driver** folder as shown in this file path description: + +``` +├── src +│ ├── printer3d_keyence_profile_capture +│ │ ├── src +│ │ | ├── driver +│ │ | ├── nodes +│ │ ├── launch +│ │ ├── config +│ │ ├── CMakeLists.txt +│ │ ├── LICENSE +│ │ ├── package.xml +│ ├── printer3d_keyence_msgs +│ ├── printer3d_keyence_profile_service_caller +├── README.md +└── .gitignore +``` + +The external library is composed of the following folders and files : + +``` +├── driver +│ ├── include +│ │ ├── LJX8_ErrorCode.h +│ │ ├── LJX8_IF_Linux.h +│ │ ├── LJXA_ACQ.h +│ ├── libsrc +│ │ ├── LJX8_IF_Linux.cpp +│ │ ├── LJXA_ACQ.cpp +│ │ ├── ProfileDataConvert.cpp +│ │ ├── ProfileDataConvert.h +│ ├── PYTHON +│ │ ├── libljxacom.so +│ │ ├── LJX8_IF.dll +│ │ ├── LJXAwrap.py +│ │ ├── Makefile +├── nodes +``` + +Once those files have been added to the **./src/printer3d_keyence_profile_capture/src/driver** folder. Open a terminal in the **./src/printer3d_keyence_profile_capture/src/driver/PYTHON** folder and make: + +``` +cd ./src/printer3d_keyence_profile_capture/src/driver/PYTHON +make +cd ../../../../../ +``` + +Once this initialisation have been done. Build the complete package and launch the service demonstrator node: + +``` +colcon build +source install/setup.bash +ros2 launch ./src/printer3d_keyence_profile_service_caller/launch/launch_keyence_commands.py +```