forked from GodotVR/TiltFiveGodot4
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add Tangible Tracking Camera support
Added enabling the tangible camera and reading frames and pose Added the T5ImageCapture node to interface with the camera
- Loading branch information
1 parent
a0207ea
commit 03c44fc
Showing
15 changed files
with
483 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,7 +1,42 @@ | ||
| extends Node3D | ||
|
|
||
| func _on_t5_manager_glasses_scene_was_added(glasses): | ||
| print("Scene ", glasses.name, " added") | ||
| var image_capture : T5ImageCapture | ||
| var is_capturing := false | ||
| var image_size : Vector2i | ||
| var camera_image : Image | ||
| @onready var texture_rect : TextureRect = $ScreenUI/CameraView | ||
|
|
||
| func _on_t5_manager_glasses_scene_will_be_removed(glasses): | ||
| print("Scene ", glasses.name, " removed") | ||
| func _on_t_5_manager_xr_rig_was_added(xr_rig : T5XRRig): | ||
| image_capture = xr_rig.get_image_capture() | ||
|
|
||
| func _on_t_5_manager_xr_rig_will_be_removed(_xr_rig): | ||
| image_capture = null | ||
|
|
||
| func _input(event): | ||
| if image_capture == null: | ||
| return | ||
| if not event.is_action_pressed("toggle_camera"): | ||
| return | ||
| if not is_capturing and image_capture.start_capture(): | ||
| is_capturing = true | ||
| texture_rect.visible = true | ||
| else: | ||
| image_capture.stop_capture() | ||
| is_capturing = false | ||
| texture_rect.visible = false | ||
|
|
||
| func _process(_delta): | ||
| if not is_capturing: | ||
| return | ||
| if image_capture.acquire_buffer(): | ||
| var buffer := image_capture.get_image_data() | ||
| var new_size = image_capture.get_image_size() | ||
| var new_illumination_mode = image_capture.get_frame_illumination_mode() | ||
| if camera_image == null or image_size != new_size: | ||
| image_size = new_size | ||
| camera_image = Image.create_from_data(image_size.x, image_size.y, false, Image.FORMAT_R8, buffer) | ||
| texture_rect.texture = ImageTexture.create_from_image(camera_image) | ||
| else: | ||
| camera_image.set_data(image_size.x, image_size.y, false, Image.FORMAT_R8, buffer) | ||
| texture_rect.texture.update(camera_image) | ||
| image_capture.release_buffer() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,143 @@ | ||
| #include <Camera.h> | ||
| #include <Logging.h> | ||
| #include <iostream> | ||
|
|
||
| namespace T5Integration { | ||
|
|
||
| extern std::mutex g_t5_exclusivity_group_1; | ||
|
|
||
| bool Camera::start_capture() { | ||
| LOG_FAIL_COND_V(_glasses.expired(), false); | ||
| _is_captured = configure_camera(true); | ||
|
|
||
| // Put the buffers into the queue for the first time.a | ||
| if (_is_captured) { | ||
| for (int i = 0; i < camera_buffer_count; ++i) { | ||
| release_filled_buffer(i); | ||
| } | ||
| } | ||
| return _is_captured; | ||
| } | ||
|
|
||
| void Camera::stop_capture() { | ||
| LOG_FAIL_COND(_glasses.expired()); | ||
| configure_camera(false); | ||
| } | ||
|
|
||
| void Camera::release_filled_buffer(int buffer_index) { | ||
| LOG_FAIL_COND(_glasses.expired()); | ||
| LOG_FAIL_COND(!_is_captured); | ||
| LOG_FAIL_COND(buffer_index < 0 || buffer_index >= camera_buffer_count); | ||
|
|
||
| auto& buffer = _camera_buffers[buffer_index]; | ||
| auto& camImage = _camera_buffer_info[buffer_index]; | ||
|
|
||
| camImage.cameraIndex = 0; | ||
| camImage.imageWidth = 0; | ||
| camImage.imageHeight = 0; | ||
| camImage.cameraIndex = 0; | ||
| camImage.imageStride = 0; | ||
| camImage.illuminationMode = 0; | ||
| camImage.bufferSize = buffer.size(); | ||
| camImage.pixelData = buffer.data(); | ||
|
|
||
| T5_Result result; | ||
| { | ||
| std::lock_guard lock(g_t5_exclusivity_group_1); | ||
| result = t5SubmitEmptyCamImageBuffer(_glasses.lock()->_glasses_handle, &camImage); | ||
| } | ||
| if (result != T5_SUCCESS) { | ||
| LOG_T5_ERROR(result); | ||
| } | ||
| } | ||
|
|
||
| int Camera::acquire_filled_buffer() { | ||
| LOG_FAIL_COND_V(_glasses.expired(), -1); | ||
| T5_CamImage camImage; | ||
| T5_Result result; | ||
| { | ||
| std::lock_guard lock(g_t5_exclusivity_group_1); | ||
| result = t5GetFilledCamImageBuffer(_glasses.lock()->_glasses_handle, &camImage); | ||
| } | ||
| if (result == T5_ERROR_TRY_AGAIN) { | ||
| return -1; | ||
| } else if (result != T5_SUCCESS) { | ||
| LOG_T5_ERROR(result); | ||
| return -1; | ||
| } | ||
| for (int i = 0; i < _camera_buffers.size(); ++i) { | ||
| if (_camera_buffers[i].data() == camImage.pixelData) { | ||
| _camera_buffer_info[i] = camImage; | ||
| return i; | ||
| } | ||
| } | ||
| LOG_ERROR("Failed to find buffer."); | ||
| return -1; | ||
| } | ||
|
|
||
| std::span<uint8_t> Camera::get_buffer(int buffer_index) { | ||
| LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffers.size(), std::span<uint8_t>()); | ||
|
|
||
| return std::span(_camera_buffers[buffer_index]); | ||
| } | ||
|
|
||
| int Camera::get_image_width(int buffer_index) const { | ||
| LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); | ||
|
|
||
| return _camera_buffer_info[buffer_index].imageWidth; | ||
| } | ||
|
|
||
| int Camera::get_image_height(int buffer_index) const { | ||
| LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); | ||
|
|
||
| return _camera_buffer_info[buffer_index].imageHeight; | ||
| } | ||
|
|
||
| int Camera::get_image_stride(int buffer_index) const { | ||
| LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), 0); | ||
|
|
||
| return _camera_buffer_info[buffer_index].imageStride; | ||
| } | ||
|
|
||
| uint8_t Camera::get_illumination_mode(int buffer_index) const { | ||
| LOG_FAIL_COND_V(buffer_index < 0 || buffer_index >= _camera_buffer_info.size(), false); | ||
|
|
||
| return _camera_buffer_info[buffer_index].illuminationMode; | ||
| } | ||
|
|
||
| void Camera::get_camera_position(int buffer_index, float& out_pos_x, float& out_pos_y, float& out_pos_z) const { | ||
| LOG_FAIL_COND(buffer_index < 0 || buffer_index >= _camera_buffer_info.size()); | ||
|
|
||
| out_pos_x = _camera_buffer_info[buffer_index].posCAM_GBD.x; | ||
| out_pos_y = _camera_buffer_info[buffer_index].posCAM_GBD.y; | ||
| out_pos_z = _camera_buffer_info[buffer_index].posCAM_GBD.z; | ||
| } | ||
|
|
||
| void Camera::get_camera_orientation(int buffer_index, float& out_quat_x, float& out_quat_y, float& out_quat_z, float& out_quat_w) const { | ||
| LOG_FAIL_COND(buffer_index < 0 || buffer_index >= _camera_buffer_info.size()); | ||
|
|
||
| out_quat_x = _camera_buffer_info[buffer_index].rotToCAM_GBD.x; | ||
| out_quat_y = _camera_buffer_info[buffer_index].rotToCAM_GBD.y; | ||
| out_quat_z = _camera_buffer_info[buffer_index].rotToCAM_GBD.z; | ||
| out_quat_w = _camera_buffer_info[buffer_index].rotToCAM_GBD.w; | ||
| } | ||
|
|
||
| bool Camera::configure_camera(bool enable) { | ||
| LOG_FAIL_COND_V(_glasses.expired(), false); | ||
| LOG_FAIL_COND_V(_camera_buffers.size() <= 0, false); | ||
|
|
||
| T5_CameraStreamConfig config{ _camera_idx, enable }; | ||
| T5_Result result = T5_SUCCESS; | ||
| { | ||
| std::lock_guard lock(g_t5_exclusivity_group_1); | ||
| result = t5ConfigureCameraStreamForGlasses(_glasses.lock()->_glasses_handle, config); | ||
| } | ||
|
|
||
| if (result != T5_SUCCESS) { | ||
| LOG_T5_ERROR(result); | ||
| return false; | ||
| } | ||
| return true; | ||
| } | ||
|
|
||
| } //namespace T5Integration |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,67 @@ | ||
| #ifndef _WANDSERVICE_H | ||
| #define _WANDSERVICE_H | ||
|
|
||
| #include <TiltFiveNative.h> | ||
| #include <array> | ||
| #include <chrono> | ||
| #include <mutex> | ||
| #include <span> | ||
| #include <thread> | ||
| #include <vector> | ||
|
|
||
| #include <Glasses.h> | ||
|
|
||
| using namespace std::chrono_literals; | ||
|
|
||
| namespace T5Integration { | ||
|
|
||
| const int camera_buffer_count = 3; | ||
| const size_t camera_buffer_size = T5_MIN_CAM_IMAGE_BUFFER_WIDTH * T5_MIN_CAM_IMAGE_BUFFER_HEIGHT; | ||
|
|
||
| using CameraBuffer = std::array<uint8_t, camera_buffer_size>; | ||
|
|
||
| class Camera { | ||
| public: | ||
| void set_camera_idx(uint8_t idx); | ||
| void set_glasses(Glasses::Ptr glasses); | ||
|
|
||
| bool start_capture(); | ||
| void stop_capture(); | ||
| bool is_capturing() const; | ||
|
|
||
| void release_filled_buffer(int buffer_index); | ||
| int acquire_filled_buffer(); | ||
| std::span<uint8_t> get_buffer(int buffer_index); | ||
| int get_image_width(int buffer_index) const; | ||
| int get_image_height(int buffer_index) const; | ||
| int get_image_stride(int buffer_index) const; | ||
| uint8_t get_illumination_mode(int buffer_index) const; | ||
| void get_camera_position(int buffer_index, float& out_pos_x, float& out_pos_y, float& out_pos_z) const; | ||
| void get_camera_orientation(int buffer_index, float& out_quat_x, float& out_quat_y, float& out_quat_z, float& out_quat_w) const; | ||
|
|
||
| private: | ||
| bool configure_camera(bool enable); | ||
|
|
||
| bool _is_captured = false; | ||
| uint8_t _camera_idx = 0; | ||
| std::weak_ptr<Glasses> _glasses; | ||
|
|
||
| std::array<CameraBuffer, camera_buffer_count> _camera_buffers; | ||
| std::array<T5_CamImage, camera_buffer_count> _camera_buffer_info; | ||
| }; | ||
|
|
||
| inline void Camera::set_camera_idx(uint8_t idx) { | ||
| _camera_idx = idx; | ||
| } | ||
|
|
||
| inline bool Camera::is_capturing() const { | ||
| return _is_captured; | ||
| } | ||
|
|
||
| inline void Camera::set_glasses(Glasses::Ptr glasses) { | ||
| _glasses = glasses; | ||
| } | ||
|
|
||
| } //namespace T5Integration | ||
|
|
||
| #endif //_WANDSERVICE_H |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.