From cf8ba42cb39fb4831e8de99d78e62e4bc5890082 Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:03:54 +0900 Subject: [PATCH 01/21] [OculusTeleop.cpp] output hand joint transformations as split strings --- app_source/Src/OculusTeleop.cpp | 43 +++++++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/app_source/Src/OculusTeleop.cpp b/app_source/Src/OculusTeleop.cpp index 6f086e6..5c19bad 100644 --- a/app_source/Src/OculusTeleop.cpp +++ b/app_source/Src/OculusTeleop.cpp @@ -1222,6 +1222,7 @@ void main() int axisSurfaces = 0; std::vector> handPoseTransformations; + std::vector> jointPoseTransformations; // add the controller model surfaces to the list of surfaces to render for (int i = 0; i < (int)InputDevices.size(); ++i) { @@ -1253,6 +1254,15 @@ void main() // TransformMatrices[axisSurfaces++] = matDeviceModel; // TransformMatrices[axisSurfaces++] = trDevice.GetPointerMatrix(); + const std::vector& joints = trDevice.GetHandModel().GetTransformedJoints(); + for (int k = 0; k < static_cast(joints.size()); ++k) { + ovrJoint joint = joints[k]; + char const *jointName = joint.Name; + OVR::Posef& jointPose = joint.Pose; + OVR::Matrix4f jointPoseMatrix = OVR::Matrix4f(jointPose); + jointPoseTransformations.push_back(std::make_pair(jointName, jointPoseMatrix)); + } + bool renderHand = (trDevice.GetHandPoseConfidence() == ovrConfidence_HIGH); if (renderHand) { trDevice.Render(out.Surfaces); @@ -1276,6 +1286,39 @@ void main() output_ss << "&" << buttons_ss.str(); __android_log_print(ANDROID_LOG_INFO, "wE9ryARX", "%s", output_ss.str().c_str()); + std::ostringstream joint_ss; + first = true; + for(auto it = std::begin(jointPoseTransformations); it != std::end(jointPoseTransformations); ++it) { + if (!first) { + joint_ss << ','; + } + char const *name = it->first; + const OVR::Matrix4f& transformationMatrix = it->second; + joint_ss << name << ":" << TransformationMatrixToString(transformationMatrix); + first = false; + } + joint_ss << "!"; // as the end flag + + int idx = 0; + std::size_t maxChunkSize = 1000; + std::string prefix = "jsk73b1z_"; + std::string longString = joint_ss.str(); + // split longString into several chunks to avoid truncation of log + for (std::size_t i = 0; i < longString.length();) { + std::size_t chunkSize = std::min(maxChunkSize, longString.length() - i); + if (i + maxChunkSize < longString.length()) { + while (longString[i + chunkSize] != ' ') // split at ' ' only + { + chunkSize--; + } + } + std::string chunk = longString.substr(i, chunkSize); + std::string s = std::to_string(idx); + __android_log_print(ANDROID_LOG_INFO, (prefix + s).c_str() , "%s", chunk.c_str()); + idx++; + i += chunkSize; + } + // Add axis if (SampleConfiguration.RenderAxis && AxisSurface.surface != nullptr) { const_cast(AxisSurface.surface)->numInstances = axisSurfaces; From 99c465a658a106aaaac11f0b5d25f878dc3e2b76 Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:42:50 +0900 Subject: [PATCH 02/21] [reader.py] process joint-transformation-string --- oculus_reader/reader.py | 98 +++++++++++++++++++++++++++++++++++++---- 1 file changed, 89 insertions(+), 9 deletions(-) diff --git a/oculus_reader/reader.py b/oculus_reader/reader.py index 2acbccf..4e0b33a 100644 --- a/oculus_reader/reader.py +++ b/oculus_reader/reader.py @@ -1,3 +1,4 @@ +#!/usr/bin/env python from oculus_reader.FPS_counter import FPSCounter from oculus_reader.buttons_parser import parse_buttons import numpy as np @@ -23,10 +24,13 @@ def __init__(self, run=True ): self.running = False - self.last_transforms = {} + self.last_hand_transforms = {} + self.last_left_joints_transforms = {} + self.last_right_joints_transforms = {} self.last_buttons = {} self._lock = threading.Lock() self.tag = 'wE9ryARX' + self.joint_tag = 'jsk73b1z_' self.ip_address = ip_address self.port = port @@ -177,22 +181,97 @@ def extract_data(self, line): pass return output + def extract_joint_data(self, line, file_obj): + output = '' + i = 0 + while not '!' in line: + if self.joint_tag in line: + tmp = line.split(self.joint_tag + str(i) + ': ') + if len(tmp) > 1: + output += tmp[1] + ' ' + i += 1 + else: + return None + line = file_obj.readline().strip() + tmp = line.split(self.joint_tag + str(i) + ': ') + if len(tmp) > 1: + output += tmp[1] + else: + return None + return output + + def process_joint_data(self, string): + try: + hand1_string, hand2_string = string.split(',WristRoot') + except ValueError: + return None, None + hand2_string = 'WristRoot' + hand2_string + hand2_string = hand2_string.split('!')[0] + + joints1_transforms = self.process_joint_transforms(hand1_string) + joints2_transforms = self.process_joint_transforms(hand2_string) + return joints1_transforms, joints2_transforms + + def process_joint_transforms(self, hand_string): + split_hand_strings = hand_string.split(' ,') + transforms = {} + for pair_string in split_hand_strings: + transform = np.empty((4,4)) + pair = pair_string.split(':') + if len(pair) != 2: + continue + joint_char = pair[0] # joint name + transform_string = pair[1] + values = transform_string.split(' ') + c = 0 + r = 0 + count = 0 + for value in values: + if not value: + continue + transform[r][c] = float(value) + c += 1 + if c >= 4: + c = 0 + r += 1 + count += 1 + if count == 16: + transforms[joint_char] = transform + return transforms + def get_transformations_and_buttons(self): with self._lock: - return self.last_transforms, self.last_buttons + return self.last_hand_transforms, self.last_buttons + + def get_joint_transformations(self): + with self._lock: + return self.last_left_joints_transforms, self.last_right_joints_transforms def read_logcat_by_line(self, connection): file_obj = connection.socket.makefile() while self.running: try: line = file_obj.readline().strip() - data = self.extract_data(line) - if data: - transforms, buttons = OculusReader.process_data(data) - with self._lock: - self.last_transforms, self.last_buttons = transforms, buttons - if self.print_FPS: - self.fps_counter.getAndPrintFPS() + if self.joint_tag in line: + data = self.extract_joint_data(line, file_obj) + if data: + joints1_transforms, joints2_transforms = self.process_joint_data(data) + if 'Thumb0' in joints1_transforms and 'Thumb0' in joints2_transforms: + with self._lock: + if joints1_transforms['Thumb0'][0][3] > 0 : + self.last_left_joints_transforms = joints1_transforms + self.last_right_joints_transforms = joints2_transforms + else: + self.last_left_joints_transforms = joints2_transforms + self.last_right_joints_transforms = joints1_transforms + else: + data = self.extract_data(line) + if data: + transforms, buttons = OculusReader.process_data(data) + with self._lock: + self.last_hand_transforms, self.last_buttons = transforms, buttons + if self.print_FPS: + self.fps_counter.getAndPrintFPS() except UnicodeDecodeError: pass file_obj.close() @@ -205,6 +284,7 @@ def main(): while True: time.sleep(0.3) print(oculus_reader.get_transformations_and_buttons()) + print(oculus_reader.get_joint_transformations()) if __name__ == '__main__': From 0f1b5875c67119bb29f3caa9f785e721d538042f Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:44:24 +0900 Subject: [PATCH 03/21] [visualize_oculus_transforms.py] publish joint tf, change base frame from world to oculus_head --- oculus_reader/visualize_oculus_transforms.py | 82 ++++++++++++++++++-- 1 file changed, 77 insertions(+), 5 deletions(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index b39a154..a788cdb 100644 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -1,9 +1,40 @@ +#!/usr/bin/env python from reader import OculusReader from tf.transformations import quaternion_from_matrix import rospy import tf2_ros import geometry_msgs.msg +import copy +import numpy as np + + +frame_tree = { + # child_frame: parent_frame + 'ForearmStub': 'WristRoot', + 'Thumb0': 'WristRoot', + 'Thumb1': 'Thumb0', + 'Thumb2': 'Thumb1', + 'Thumb3': 'Thumb2', + 'Index1': 'WristRoot', + 'Index2': 'Index1', + 'Index3': 'Index2', + 'Middle1': 'WristRoot', + 'Middle2': 'Middle1', + 'Middle3': 'Middle2', + 'Ring1': 'WristRoot', + 'Ring2': 'Ring1', + 'Ring3': 'Ring2', + 'Pinky0': 'WristRoot', + 'Pinky1': 'Pinky0', + 'Pinky2': 'Pinky1', + 'Pinky3': 'Pinky2', + 'ThumbTip': 'Thumb3', + 'IndexTip': 'Index3', + 'MiddleTip': 'Middle3', + 'RingTip': 'Ring3', + 'PinkyTip': 'Pinky3', +} def publish_transform(transform, name): translation = transform[:3, 3] @@ -12,7 +43,7 @@ def publish_transform(transform, name): t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() - t.header.frame_id = 'world' + t.header.frame_id = 'oculus_head' t.child_frame_id = name t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] @@ -26,20 +57,61 @@ def publish_transform(transform, name): br.sendTransform(t) +def publish_joint_transform(transform, name, prefix=''): + translation = transform[:3, 3] + + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + t.header.stamp = rospy.Time.now() + if 'WristRoot' in name: + t.header.frame_id = prefix + 'palm' + else: + t.header.frame_id = prefix + frame_tree[name] + t.child_frame_id = prefix + name + t.transform.translation.x = translation[0] + t.transform.translation.y = translation[1] + t.transform.translation.z = translation[2] + + quat = quaternion_from_matrix(transform) + t.transform.rotation.x = quat[0] + t.transform.rotation.y = quat[1] + t.transform.rotation.z = quat[2] + t.transform.rotation.w = quat[3] + + br.sendTransform(t) + +def process_transformations(transformations): + tmp_transformations = copy.deepcopy(transformations) + for name, transform in transformations.items(): + if name in frame_tree: + parent_frame = frame_tree[name] + parent_transform = transformations[parent_frame] + tmp_transformations[name] = np.matmul(np.linalg.inv(parent_transform), transform) + return tmp_transformations def main(): oculus_reader = OculusReader() rospy.init_node('oculus_reader') - while not rospy.is_shutdown(): rospy.sleep(1) transformations, buttons = oculus_reader.get_transformations_and_buttons() - if 'r' not in transformations: + if 'l' not in transformations or 'r' not in transformations: continue + left_controller_pose = transformations['l'] + publish_transform(left_controller_pose, 'l_palm') right_controller_pose = transformations['r'] - publish_transform(right_controller_pose, 'oculus') - print('transformations', transformations) + publish_transform(right_controller_pose, 'r_palm') + + left_joint_transformations, right_joint_transformations = oculus_reader.get_joint_transformations() + left_joint_transformations = process_transformations(left_joint_transformations) + right_joint_transformations = process_transformations(right_joint_transformations) + for joint_name, transform in left_joint_transformations.items(): + publish_joint_transform(transform, joint_name, prefix='l_') + for joint_name, transform in right_joint_transformations.items(): + publish_joint_transform(transform, joint_name, prefix='r_') + print('buttons', buttons) if __name__ == '__main__': From 5a445ed3df7b2637373bc0333204d8da6a31d2ab Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:45:29 +0900 Subject: [PATCH 04/21] [APK] update apk to support publishing hand-joint-transformations --- oculus_reader/APK/teleop-debug.apk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) mode change 100755 => 100644 oculus_reader/APK/teleop-debug.apk diff --git a/oculus_reader/APK/teleop-debug.apk b/oculus_reader/APK/teleop-debug.apk old mode 100755 new mode 100644 index f00f2b1..623d630 --- a/oculus_reader/APK/teleop-debug.apk +++ b/oculus_reader/APK/teleop-debug.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:97b49f94682a732e14d131d50bcc7885e183d73b977e19a1752c28502519cd4b -size 4865715 +oid sha256:bc9bda48a285812fc3b821386cb6d8f2e0902d57a00bc464aec398ec492c1ddb +size 4890358 From a95437a92af2f50e4dccdf4ec046d7aff4a43a3b Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:46:50 +0900 Subject: [PATCH 05/21] [reader.py] reinstall apk every time --- oculus_reader/reader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/oculus_reader/reader.py b/oculus_reader/reader.py index 4e0b33a..dec7ad9 100644 --- a/oculus_reader/reader.py +++ b/oculus_reader/reader.py @@ -40,7 +40,7 @@ def __init__(self, self.fps_counter = FPSCounter() self.device = self.get_device() - self.install(verbose=False) + self.install(verbose=False, reinstall=True) if run: self.run() From ee3219a5bee4dfabce6161c89995338106992b8c Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 21:58:10 +0900 Subject: [PATCH 06/21] [visualize_oculus_transforms.py] allow executing as program --- oculus_reader/visualize_oculus_transforms.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 oculus_reader/visualize_oculus_transforms.py diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py old mode 100644 new mode 100755 From 300aadc338a85e5e4ad542f4adb22fe6a8996f29 Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 22:01:38 +0900 Subject: [PATCH 07/21] [reader.py] judge if joints_transforms is None instead of checking if Thumb0 inside joints_transforms --- oculus_reader/reader.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/oculus_reader/reader.py b/oculus_reader/reader.py index dec7ad9..5bd84cd 100644 --- a/oculus_reader/reader.py +++ b/oculus_reader/reader.py @@ -256,7 +256,7 @@ def read_logcat_by_line(self, connection): data = self.extract_joint_data(line, file_obj) if data: joints1_transforms, joints2_transforms = self.process_joint_data(data) - if 'Thumb0' in joints1_transforms and 'Thumb0' in joints2_transforms: + if joints1_transforms is not None and joints2_transforms is not None: with self._lock: if joints1_transforms['Thumb0'][0][3] > 0 : self.last_left_joints_transforms = joints1_transforms From cecf866179965f1ed1171294eb56d864447473ca Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 22:02:29 +0900 Subject: [PATCH 08/21] set oculus_reader as a ros package --- CMakeLists.txt | 12 ++++++++++++ package.xml | 16 ++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b058a55 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,12 @@ +cmake_minimum_required(VERSION 3.0.2) +project(oculus_reader) + +find_package(catkin REQUIRED COMPONENTS + rospy + geometry_msgs + tf2_ros +) + +catkin_package( + CATKIN_DEPENDS geometry_msgs +) diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..34fdc86 --- /dev/null +++ b/package.xml @@ -0,0 +1,16 @@ + + + oculus_reader + 0.0.1 + The oculus_reader package + + TODO + + Apache-2 + + catkin + + rospy + geometry_msgs + tf2_ros + From 7357cb13a3d91133050957a34f0c6c49003144ef Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 22:04:57 +0900 Subject: [PATCH 09/21] [visualize_oculus_transforms] get rate from rosparam --- oculus_reader/visualize_oculus_transforms.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index a788cdb..9f2cf5c 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -93,8 +93,9 @@ def process_transformations(transformations): def main(): oculus_reader = OculusReader() rospy.init_node('oculus_reader') + rate = rospy.get_param('~rate', 1.0) while not rospy.is_shutdown(): - rospy.sleep(1) + rospy.sleep(1.0 / rate) transformations, buttons = oculus_reader.get_transformations_and_buttons() if 'l' not in transformations or 'r' not in transformations: continue From 23884d965db528dfc2f27881a9abaa7c39d28819 Mon Sep 17 00:00:00 2001 From: wu Date: Fri, 1 Dec 2023 22:08:00 +0900 Subject: [PATCH 10/21] add launch file and corresponding rviz config file --- config/oculus_reader.rviz | 304 ++++++++++++++++++++++++++++++++++++++ launch/exe.launch | 9 ++ 2 files changed, 313 insertions(+) create mode 100644 config/oculus_reader.rviz create mode 100644 launch/exe.launch diff --git a/config/oculus_reader.rviz b/config/oculus_reader.rviz new file mode 100644 index 0000000..2561cfd --- /dev/null +++ b/config/oculus_reader.rviz @@ -0,0 +1,304 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + Splitter Ratio: 0.5 + Tree Height: 1079 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Class: rviz/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + l_ForearmStub: + Value: true + l_Index1: + Value: true + l_Index2: + Value: true + l_Index3: + Value: true + l_IndexTip: + Value: true + l_Middle1: + Value: true + l_Middle2: + Value: true + l_Middle3: + Value: true + l_MiddleTip: + Value: true + l_Pinky0: + Value: true + l_Pinky1: + Value: true + l_Pinky2: + Value: true + l_Pinky3: + Value: true + l_PinkyTip: + Value: true + l_Ring1: + Value: true + l_Ring2: + Value: true + l_Ring3: + Value: true + l_RingTip: + Value: true + l_Thumb0: + Value: true + l_Thumb1: + Value: true + l_Thumb2: + Value: true + l_Thumb3: + Value: true + l_ThumbTip: + Value: true + l_WristRoot: + Value: true + l_palm: + Value: true + oculus_head: + Value: true + r_ForearmStub: + Value: true + r_Index1: + Value: true + r_Index2: + Value: true + r_Index3: + Value: true + r_IndexTip: + Value: true + r_Middle1: + Value: true + r_Middle2: + Value: true + r_Middle3: + Value: true + r_MiddleTip: + Value: true + r_Pinky0: + Value: true + r_Pinky1: + Value: true + r_Pinky2: + Value: true + r_Pinky3: + Value: true + r_PinkyTip: + Value: true + r_Ring1: + Value: true + r_Ring2: + Value: true + r_Ring3: + Value: true + r_RingTip: + Value: true + r_Thumb0: + Value: true + r_Thumb1: + Value: true + r_Thumb2: + Value: true + r_Thumb3: + Value: true + r_ThumbTip: + Value: true + r_WristRoot: + Value: true + r_palm: + Value: true + world: + Value: true + Marker Alpha: 1 + Marker Scale: 0.10000000149011612 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + world: + oculus_head: + l_palm: + l_WristRoot: + l_ForearmStub: + {} + l_Index1: + l_Index2: + l_Index3: + l_IndexTip: + {} + l_Middle1: + l_Middle2: + l_Middle3: + l_MiddleTip: + {} + l_Pinky0: + l_Pinky1: + l_Pinky2: + l_Pinky3: + l_PinkyTip: + {} + l_Ring1: + l_Ring2: + l_Ring3: + l_RingTip: + {} + l_Thumb0: + l_Thumb1: + l_Thumb2: + l_Thumb3: + l_ThumbTip: + {} + r_palm: + r_WristRoot: + r_ForearmStub: + {} + r_Index1: + r_Index2: + r_Index3: + r_IndexTip: + {} + r_Middle1: + r_Middle2: + r_Middle3: + r_MiddleTip: + {} + r_Pinky0: + r_Pinky1: + r_Pinky2: + r_Pinky3: + r_PinkyTip: + {} + r_Ring1: + r_Ring2: + r_Ring3: + r_RingTip: + {} + r_Thumb0: + r_Thumb1: + r_Thumb2: + r_Thumb3: + r_ThumbTip: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 0.7703558206558228 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.0597972869873047 + Target Frame: + Yaw: 4.338580131530762 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1376 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000004c2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000004c2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000004c2000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a00000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 2560 + X: 0 + Y: 27 diff --git a/launch/exe.launch b/launch/exe.launch new file mode 100644 index 0000000..67348dc --- /dev/null +++ b/launch/exe.launch @@ -0,0 +1,9 @@ + + + + + + + + From 7cff7b8919319930e1219042b236150cb98c5da1 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 6 Dec 2023 13:14:40 +0900 Subject: [PATCH 11/21] [visualize_oculus_transforms.py] check if matrix is invertible before inverting to avoid problem caused by singular matrix when no hand is detected --- oculus_reader/visualize_oculus_transforms.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index 9f2cf5c..3f41601 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -87,7 +87,10 @@ def process_transformations(transformations): if name in frame_tree: parent_frame = frame_tree[name] parent_transform = transformations[parent_frame] - tmp_transformations[name] = np.matmul(np.linalg.inv(parent_transform), transform) + if np.linalg.cond(parent_transform) < 1/np.finfo(parent_transform.dtype).eps: + tmp_transformations[name] = np.matmul(np.linalg.inv(parent_transform), transform) + else: + tmp_transformations[name] = np.eye(4) return tmp_transformations def main(): From 43b89dde6726a3a830dfd98f01943e974b0d71d9 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 13 Dec 2023 16:28:59 +0900 Subject: [PATCH 12/21] [OculusTeleop.cpp] get finger joint pose only if hands are tracked instead of controllers --- app_source/Src/OculusTeleop.cpp | 27 +++++++++++++++------------ oculus_reader/APK/teleop-debug.apk | 4 ++-- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/app_source/Src/OculusTeleop.cpp b/app_source/Src/OculusTeleop.cpp index 5c19bad..2490ede 100644 --- a/app_source/Src/OculusTeleop.cpp +++ b/app_source/Src/OculusTeleop.cpp @@ -1254,13 +1254,16 @@ void main() // TransformMatrices[axisSurfaces++] = matDeviceModel; // TransformMatrices[axisSurfaces++] = trDevice.GetPointerMatrix(); - const std::vector& joints = trDevice.GetHandModel().GetTransformedJoints(); - for (int k = 0; k < static_cast(joints.size()); ++k) { - ovrJoint joint = joints[k]; - char const *jointName = joint.Name; - OVR::Posef& jointPose = joint.Pose; - OVR::Matrix4f jointPoseMatrix = OVR::Matrix4f(jointPose); - jointPoseTransformations.push_back(std::make_pair(jointName, jointPoseMatrix)); + // get joint pose only if the device is a hand + if (device->GetType() == ovrControllerType_Hand) { + const std::vector& joints = trDevice.GetHandModel().GetTransformedJoints(); + for (int k = 0; k < static_cast(joints.size()); ++k) { + ovrJoint joint = joints[k]; + char const *jointName = joint.Name; + OVR::Posef &jointPose = joint.Pose; + OVR::Matrix4f jointPoseMatrix = OVR::Matrix4f(jointPose); + jointPoseTransformations.push_back(std::make_pair(jointName, jointPoseMatrix)); + } } bool renderHand = (trDevice.GetHandPoseConfidence() == ovrConfidence_HIGH); @@ -1272,7 +1275,7 @@ void main() // send values std::ostringstream output_ss, buttons_ss; bool first = true; - for(auto it = std::begin(handPoseTransformations); it != std::end(handPoseTransformations); ++it) { + for (auto it = std::begin(handPoseTransformations); it != std::end(handPoseTransformations); ++it) { if (!first) { output_ss << '|'; buttons_ss << ","; @@ -1288,7 +1291,7 @@ void main() std::ostringstream joint_ss; first = true; - for(auto it = std::begin(jointPoseTransformations); it != std::end(jointPoseTransformations); ++it) { + for (auto it = std::begin(jointPoseTransformations); it != std::end(jointPoseTransformations); ++it) { if (!first) { joint_ss << ','; } @@ -1307,14 +1310,14 @@ void main() for (std::size_t i = 0; i < longString.length();) { std::size_t chunkSize = std::min(maxChunkSize, longString.length() - i); if (i + maxChunkSize < longString.length()) { - while (longString[i + chunkSize] != ' ') // split at ' ' only - { + // split at ' ' only + while (longString[i + chunkSize] != ' ') { chunkSize--; } } std::string chunk = longString.substr(i, chunkSize); std::string s = std::to_string(idx); - __android_log_print(ANDROID_LOG_INFO, (prefix + s).c_str() , "%s", chunk.c_str()); + __android_log_print(ANDROID_LOG_INFO, (prefix + s).c_str(), "%s", chunk.c_str()); idx++; i += chunkSize; } diff --git a/oculus_reader/APK/teleop-debug.apk b/oculus_reader/APK/teleop-debug.apk index 623d630..7e9e71b 100644 --- a/oculus_reader/APK/teleop-debug.apk +++ b/oculus_reader/APK/teleop-debug.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:bc9bda48a285812fc3b821386cb6d8f2e0902d57a00bc464aec398ec492c1ddb -size 4890358 +oid sha256:3a4d4f9203bb58c1bab8b316cf051e59f3e1dcefe0739bc66a89d125d70aab9a +size 4890269 From 698b11a44af127743205de7f8ec22368faf84c01 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 13 Dec 2023 16:30:34 +0900 Subject: [PATCH 13/21] [reader/visualize_oculus_transforms] stop publishing joint tf when controllers are tracked --- oculus_reader/reader.py | 9 +++++ oculus_reader/visualize_oculus_transforms.py | 42 ++++++++++---------- 2 files changed, 30 insertions(+), 21 deletions(-) diff --git a/oculus_reader/reader.py b/oculus_reader/reader.py index 5bd84cd..200fbe2 100644 --- a/oculus_reader/reader.py +++ b/oculus_reader/reader.py @@ -229,6 +229,10 @@ def process_joint_transforms(self, hand_string): for value in values: if not value: continue + # when switch tracked device from hand to controller + # the first transform is likely to be invalid with nan values + if np.isnan(float(value)): + return None transform[r][c] = float(value) c += 1 if c >= 4: @@ -264,6 +268,11 @@ def read_logcat_by_line(self, connection): else: self.last_left_joints_transforms = joints2_transforms self.last_right_joints_transforms = joints1_transforms + else: + with self._lock: + # clear history + self.last_left_joints_transforms = {} + self.last_right_joints_transforms = {} else: data = self.extract_data(line) if data: diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index 3f41601..2ead27f 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -37,32 +37,32 @@ } def publish_transform(transform, name): - translation = transform[:3, 3] - - br = tf2_ros.TransformBroadcaster() - t = geometry_msgs.msg.TransformStamped() - - t.header.stamp = rospy.Time.now() - t.header.frame_id = 'oculus_head' - t.child_frame_id = name - t.transform.translation.x = translation[0] - t.transform.translation.y = translation[1] - t.transform.translation.z = translation[2] - - quat = quaternion_from_matrix(transform) - t.transform.rotation.x = quat[0] - t.transform.rotation.y = quat[1] - t.transform.rotation.z = quat[2] - t.transform.rotation.w = quat[3] - - br.sendTransform(t) + # when switch tracked device from hand to controller, the first transform is likely to be invalid + if np.isclose(np.linalg.det(transform[:3, :3]), 1.0, atol=1e-3): + br = tf2_ros.TransformBroadcaster() + t = geometry_msgs.msg.TransformStamped() + + translation = transform[:3, 3] + t.header.stamp = rospy.Time.now() + t.header.frame_id = 'oculus_head' + t.child_frame_id = name + t.transform.translation.x = translation[0] + t.transform.translation.y = translation[1] + t.transform.translation.z = translation[2] + + quat = quaternion_from_matrix(transform) + t.transform.rotation.x = quat[0] + t.transform.rotation.y = quat[1] + t.transform.rotation.z = quat[2] + t.transform.rotation.w = quat[3] + + br.sendTransform(t) def publish_joint_transform(transform, name, prefix=''): - translation = transform[:3, 3] - br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() + translation = transform[:3, 3] t.header.stamp = rospy.Time.now() if 'WristRoot' in name: t.header.frame_id = prefix + 'palm' From caaf7f36193ec8463af73746de6bd4ce1e9cf6f9 Mon Sep 17 00:00:00 2001 From: wu Date: Wed, 13 Dec 2023 21:02:12 +0900 Subject: [PATCH 14/21] [visualize_oculus_transforms.py] separately publish button data to each of the corresponding ros topic --- oculus_reader/visualize_oculus_transforms.py | 32 +++++++++++++++++++- 1 file changed, 31 insertions(+), 1 deletion(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index 2ead27f..c0f240b 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -4,6 +4,7 @@ import rospy import tf2_ros import geometry_msgs.msg +from std_msgs.msg import Bool, Float32, Float32MultiArray import copy import numpy as np @@ -93,10 +94,38 @@ def process_transformations(transformations): tmp_transformations[name] = np.eye(4) return tmp_transformations +def init_button_publisher(): + namelist = ['A', 'B', 'RThU', 'RJ', 'RG', 'RTr', 'rightJS', 'rightGrip', 'rightTrig', + 'X', 'Y', 'LThU', 'LJ', 'LG', 'LTr', 'leftJS', 'leftGrip', 'leftTrig'] + typelist = [Bool, Bool, Bool, Bool, Bool, Bool, Float32MultiArray, Float32, Float32, + Bool, Bool, Bool, Bool, Bool, Bool, Float32MultiArray, Float32, Float32] + prefix = '/oculus_reader/' + topiclist = [prefix + name for name in namelist] + publishers = {} + for idx, (topic, msg_type) in enumerate(zip(topiclist, typelist)): + publishers[namelist[idx]] = rospy.Publisher(topic, msg_type, queue_size=1) + return publishers + +def publish_buttons(buttons, publishers): + for key, value in buttons.items(): + if key in publishers: + if isinstance(value, bool): + msg = Bool() + elif isinstance(value, tuple): + if len(value) == 1: + msg = Float32() + value = value[0] + else: + msg = Float32MultiArray() + value = list(value) + msg.data = value + publishers[key].publish(msg) + def main(): oculus_reader = OculusReader() rospy.init_node('oculus_reader') rate = rospy.get_param('~rate', 1.0) + button_publishers = init_button_publisher() while not rospy.is_shutdown(): rospy.sleep(1.0 / rate) transformations, buttons = oculus_reader.get_transformations_and_buttons() @@ -116,7 +145,8 @@ def main(): for joint_name, transform in right_joint_transformations.items(): publish_joint_transform(transform, joint_name, prefix='r_') - print('buttons', buttons) + if len(left_joint_transformations) == 0: + publish_buttons(buttons, button_publishers) if __name__ == '__main__': main() From af8e719c8ac5fc8bfe445d9bfcf0f2f8f8fe754d Mon Sep 17 00:00:00 2001 From: wu Date: Thu, 14 Dec 2023 17:13:49 +0900 Subject: [PATCH 15/21] [OculusTeleop] get initial head pose and track head movements --- app_source/Src/OculusTeleop.cpp | 23 ++++++++++++++++++++--- app_source/Src/OculusTeleop.h | 2 ++ 2 files changed, 22 insertions(+), 3 deletions(-) diff --git a/app_source/Src/OculusTeleop.cpp b/app_source/Src/OculusTeleop.cpp index 2490ede..97b3c6c 100644 --- a/app_source/Src/OculusTeleop.cpp +++ b/app_source/Src/OculusTeleop.cpp @@ -531,6 +531,7 @@ void main() ControllerModelOculusTouchRight(nullptr), Buttons(nullptr) { theApp = this; + InitHeadMatrixInvPtr = nullptr; } //============================== @@ -1221,7 +1222,7 @@ void main() BeamRenderer->RenderEyeView(out.FrameMatrices.CenterView, projectionMatrix, out.Surfaces); int axisSurfaces = 0; - std::vector> handPoseTransformations; + std::vector> handHeadPoseTransformations; std::vector> jointPoseTransformations; // add the controller model surfaces to the list of surfaces to render @@ -1240,6 +1241,16 @@ void main() continue; } ovrInputDeviceHandBase& trDevice = *static_cast(device); + + // get Initial Head Pose Matrix (Inverted for further calculation) + if (InitHeadMatrixInvPtr == nullptr) { + const Posef& headPose = trDevice.GetHeadPose(); + const OVR::Matrix4f headPoseMatrix = OVR::Matrix4f(headPose); + OVR::Matrix4f tmpMatrix(Matrix4f::NoInit); + tmpMatrix = headPoseMatrix.Inverted(); + InitHeadMatrixInvPtr = new OVR::Matrix4f(tmpMatrix); + } + const char side = trDevice.IsLeftHand() ? 'l' : 'r'; const Posef& handPose = trDevice.GetHandPose(); @@ -1249,8 +1260,14 @@ void main() const OVR::Matrix4f headPoseMatrix = OVR::Matrix4f(headPose); OVR::Matrix4f handPoseMatrixHeadCoord(Matrix4f::NoInit); OVR::Matrix4f::Multiply(&handPoseMatrixHeadCoord, headPoseMatrix.Inverted(), handPoseMatrix); - handPoseTransformations.push_back(std::make_pair(side, handPoseMatrixHeadCoord)); + handHeadPoseTransformations.push_back(std::make_pair(side, handPoseMatrixHeadCoord)); TransformMatrices[axisSurfaces++] = handPoseMatrix; + + if (i == 0) { + OVR::Matrix4f headPoseMatrixInitHeadCoord(Matrix4f::NoInit); + OVR::Matrix4f::Multiply(&headPoseMatrixInitHeadCoord, *InitHeadMatrixInvPtr, headPoseMatrix); + handHeadPoseTransformations.push_back(std::make_pair('h', headPoseMatrixInitHeadCoord)); + } // TransformMatrices[axisSurfaces++] = matDeviceModel; // TransformMatrices[axisSurfaces++] = trDevice.GetPointerMatrix(); @@ -1275,7 +1292,7 @@ void main() // send values std::ostringstream output_ss, buttons_ss; bool first = true; - for (auto it = std::begin(handPoseTransformations); it != std::end(handPoseTransformations); ++it) { + for (auto it = std::begin(handHeadPoseTransformations); it != std::end(handHeadPoseTransformations); ++it) { if (!first) { output_ss << '|'; buttons_ss << ","; diff --git a/app_source/Src/OculusTeleop.h b/app_source/Src/OculusTeleop.h index c92dc81..0706ad8 100644 --- a/app_source/Src/OculusTeleop.h +++ b/app_source/Src/OculusTeleop.h @@ -552,6 +552,8 @@ namespace OVRFW { GlBuffer AxisUniformBuffer; GlProgram ProgAxis; + OVR::Matrix4f* InitHeadMatrixInvPtr; + // because a single Gear VR controller can be a left or right controller dependent on the // user's handedness (dominant hand) setting, we can't simply track controllers using a left // or right hand slot look up, because on any frame a Gear VR controller could change from From 8f3ff746893c38197d48626056999ae0bbdf2f06 Mon Sep 17 00:00:00 2001 From: wu Date: Thu, 14 Dec 2023 17:14:05 +0900 Subject: [PATCH 16/21] update APK --- oculus_reader/APK/teleop-debug.apk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/oculus_reader/APK/teleop-debug.apk b/oculus_reader/APK/teleop-debug.apk index 7e9e71b..08673a3 100644 --- a/oculus_reader/APK/teleop-debug.apk +++ b/oculus_reader/APK/teleop-debug.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:3a4d4f9203bb58c1bab8b316cf051e59f3e1dcefe0739bc66a89d125d70aab9a -size 4890269 +oid sha256:e7359cd23841c3c064b51a9bdf11a1aa9337ebe9910e8bb948dc50123be85aac +size 4890470 From 0abd798369a3d63592996af2502d8ecb8e327d79 Mon Sep 17 00:00:00 2001 From: wu Date: Thu, 14 Dec 2023 17:15:24 +0900 Subject: [PATCH 17/21] [visualize_oculus_transforms] publish oculus_head tf frame relative to init_head tf frame --- oculus_reader/visualize_oculus_transforms.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index c0f240b..d9ea051 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -37,7 +37,7 @@ 'PinkyTip': 'Pinky3', } -def publish_transform(transform, name): +def publish_transform(transform, name, parent_frame): # when switch tracked device from hand to controller, the first transform is likely to be invalid if np.isclose(np.linalg.det(transform[:3, :3]), 1.0, atol=1e-3): br = tf2_ros.TransformBroadcaster() @@ -45,7 +45,7 @@ def publish_transform(transform, name): translation = transform[:3, 3] t.header.stamp = rospy.Time.now() - t.header.frame_id = 'oculus_head' + t.header.frame_id = parent_frame t.child_frame_id = name t.transform.translation.x = translation[0] t.transform.translation.y = translation[1] @@ -132,10 +132,13 @@ def main(): if 'l' not in transformations or 'r' not in transformations: continue + head_pose = transformations['h'] + publish_transform(head_pose, 'oculus_head', 'init_head') + left_controller_pose = transformations['l'] - publish_transform(left_controller_pose, 'l_palm') + publish_transform(left_controller_pose, 'l_palm', 'oculus_head') right_controller_pose = transformations['r'] - publish_transform(right_controller_pose, 'r_palm') + publish_transform(right_controller_pose, 'r_palm', 'oculus_head') left_joint_transformations, right_joint_transformations = oculus_reader.get_joint_transformations() left_joint_transformations = process_transformations(left_joint_transformations) From 004761aec2bc970a58738e333186286c1d7f12ac Mon Sep 17 00:00:00 2001 From: wu Date: Thu, 14 Dec 2023 17:18:58 +0900 Subject: [PATCH 18/21] [exe.launch] publish init_head (instead of oculus_head) tf relative to world, set default world rotation with x pointing forward, z upward --- launch/exe.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/exe.launch b/launch/exe.launch index 67348dc..e284e32 100644 --- a/launch/exe.launch +++ b/launch/exe.launch @@ -1,6 +1,6 @@ + args="0 0 0 -1.570796 0 1.570796 world init_head 100" /> From f70b25bd3563d4652165eb66ce577f8bb3eaca23 Mon Sep 17 00:00:00 2001 From: wu Date: Thu, 14 Dec 2023 17:20:03 +0900 Subject: [PATCH 19/21] [config] update --- config/oculus_reader.rviz | 128 ++++++++++++++++++++------------------ 1 file changed, 66 insertions(+), 62 deletions(-) diff --git a/config/oculus_reader.rviz b/config/oculus_reader.rviz index 2561cfd..6cc2bb8 100644 --- a/config/oculus_reader.rviz +++ b/config/oculus_reader.rviz @@ -7,6 +7,7 @@ Panels: - /Global Options1 - /Status1 - /TF1 + - /TF1/Frames1 Splitter Ratio: 0.5 Tree Height: 1079 - Class: rviz/Selection @@ -59,6 +60,8 @@ Visualization Manager: Frame Timeout: 15 Frames: All Enabled: true + init_head: + Value: true l_ForearmStub: Value: true l_Index1: @@ -171,69 +174,70 @@ Visualization Manager: Show Names: true Tree: world: - oculus_head: - l_palm: - l_WristRoot: - l_ForearmStub: - {} - l_Index1: - l_Index2: - l_Index3: - l_IndexTip: - {} - l_Middle1: - l_Middle2: - l_Middle3: - l_MiddleTip: - {} - l_Pinky0: - l_Pinky1: - l_Pinky2: - l_Pinky3: - l_PinkyTip: + init_head: + oculus_head: + l_palm: + l_WristRoot: + l_ForearmStub: + {} + l_Index1: + l_Index2: + l_Index3: + l_IndexTip: + {} + l_Middle1: + l_Middle2: + l_Middle3: + l_MiddleTip: + {} + l_Pinky0: + l_Pinky1: + l_Pinky2: + l_Pinky3: + l_PinkyTip: + {} + l_Ring1: + l_Ring2: + l_Ring3: + l_RingTip: {} - l_Ring1: - l_Ring2: - l_Ring3: - l_RingTip: - {} - l_Thumb0: - l_Thumb1: - l_Thumb2: - l_Thumb3: - l_ThumbTip: + l_Thumb0: + l_Thumb1: + l_Thumb2: + l_Thumb3: + l_ThumbTip: + {} + r_palm: + r_WristRoot: + r_ForearmStub: + {} + r_Index1: + r_Index2: + r_Index3: + r_IndexTip: {} - r_palm: - r_WristRoot: - r_ForearmStub: - {} - r_Index1: - r_Index2: - r_Index3: - r_IndexTip: - {} - r_Middle1: - r_Middle2: - r_Middle3: - r_MiddleTip: - {} - r_Pinky0: - r_Pinky1: - r_Pinky2: - r_Pinky3: - r_PinkyTip: + r_Middle1: + r_Middle2: + r_Middle3: + r_MiddleTip: {} - r_Ring1: - r_Ring2: - r_Ring3: - r_RingTip: - {} - r_Thumb0: - r_Thumb1: - r_Thumb2: - r_Thumb3: - r_ThumbTip: + r_Pinky0: + r_Pinky1: + r_Pinky2: + r_Pinky3: + r_PinkyTip: + {} + r_Ring1: + r_Ring2: + r_Ring3: + r_RingTip: {} + r_Thumb0: + r_Thumb1: + r_Thumb2: + r_Thumb3: + r_ThumbTip: + {} Update Interval: 0 Value: true Enabled: true @@ -264,7 +268,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 0.7703558206558228 + Distance: 1.0362094640731812 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -280,9 +284,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.0597972869873047 + Pitch: 1.134797215461731 Target Frame: - Yaw: 4.338580131530762 + Yaw: 2.953577995300293 Saved: ~ Window Geometry: Displays: From e46a039b8f683653ad8e406cb6aabaf50f0fd363 Mon Sep 17 00:00:00 2001 From: wu Date: Sun, 24 Dec 2023 20:44:48 +0900 Subject: [PATCH 20/21] [visualize_oculus_transforms.py] solve 'KeyError: h' by checking if key h exists inside transformation dictionary --- oculus_reader/visualize_oculus_transforms.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py index d9ea051..f24a3f5 100755 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -129,7 +129,7 @@ def main(): while not rospy.is_shutdown(): rospy.sleep(1.0 / rate) transformations, buttons = oculus_reader.get_transformations_and_buttons() - if 'l' not in transformations or 'r' not in transformations: + if 'l' not in transformations or 'r' not in transformations or 'h' not in transformations: continue head_pose = transformations['h'] From f821f81275872022f7bdd701924ed13ff5b66910 Mon Sep 17 00:00:00 2001 From: wu Date: Sun, 24 Dec 2023 20:54:32 +0900 Subject: [PATCH 21/21] [config/oculus_reader.rviz] change frame timeout from 15 to 1 --- config/oculus_reader.rviz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/oculus_reader.rviz b/config/oculus_reader.rviz index 6cc2bb8..2932fc8 100644 --- a/config/oculus_reader.rviz +++ b/config/oculus_reader.rviz @@ -57,7 +57,7 @@ Visualization Manager: Enabled: true Filter (blacklist): "" Filter (whitelist): "" - Frame Timeout: 15 + Frame Timeout: 1 Frames: All Enabled: true init_head: