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
+```