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/app_source/Src/OculusTeleop.cpp b/app_source/Src/OculusTeleop.cpp index 6f086e6..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,8 @@ 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 for (int i = 0; i < (int)InputDevices.size(); ++i) { @@ -1239,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(); @@ -1248,11 +1260,29 @@ 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(); + // 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); if (renderHand) { trDevice.Render(out.Surfaces); @@ -1262,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 << ","; @@ -1276,6 +1306,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()) { + // 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()); + idx++; + i += chunkSize; + } + // Add axis if (SampleConfiguration.RenderAxis && AxisSurface.surface != nullptr) { const_cast(AxisSurface.surface)->numInstances = axisSurfaces; 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 diff --git a/config/oculus_reader.rviz b/config/oculus_reader.rviz new file mode 100644 index 0000000..2932fc8 --- /dev/null +++ b/config/oculus_reader.rviz @@ -0,0 +1,308 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1 + - /TF1/Frames1 + 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: 1 + Frames: + All Enabled: true + init_head: + Value: 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: + 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_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: 1.0362094640731812 + 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.134797215461731 + Target Frame: + Yaw: 2.953577995300293 + 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..e284e32 --- /dev/null +++ b/launch/exe.launch @@ -0,0 +1,9 @@ + + + + + + + + diff --git a/oculus_reader/APK/teleop-debug.apk b/oculus_reader/APK/teleop-debug.apk old mode 100755 new mode 100644 index f00f2b1..08673a3 --- 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:e7359cd23841c3c064b51a9bdf11a1aa9337ebe9910e8bb948dc50123be85aac +size 4890470 diff --git a/oculus_reader/reader.py b/oculus_reader/reader.py index 2acbccf..200fbe2 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 @@ -36,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() @@ -177,22 +181,106 @@ 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 + # 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: + 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 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 + self.last_right_joints_transforms = joints2_transforms + 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: + 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 +293,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__': diff --git a/oculus_reader/visualize_oculus_transforms.py b/oculus_reader/visualize_oculus_transforms.py old mode 100644 new mode 100755 index b39a154..f24a3f5 --- a/oculus_reader/visualize_oculus_transforms.py +++ b/oculus_reader/visualize_oculus_transforms.py @@ -1,19 +1,75 @@ +#!/usr/bin/env python from reader import OculusReader from tf.transformations import quaternion_from_matrix import rospy import tf2_ros import geometry_msgs.msg +from std_msgs.msg import Bool, Float32, Float32MultiArray +import copy +import numpy as np -def publish_transform(transform, name): - translation = transform[:3, 3] +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, 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() + t = geometry_msgs.msg.TransformStamped() + + translation = transform[:3, 3] + t.header.stamp = rospy.Time.now() + t.header.frame_id = parent_frame + 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=''): br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() + translation = transform[:3, 3] t.header.stamp = rospy.Time.now() - t.header.frame_id = 'world' - t.child_frame_id = name + 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] @@ -26,21 +82,74 @@ def publish_transform(transform, name): 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] + 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 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) + rospy.sleep(1.0 / rate) transformations, buttons = oculus_reader.get_transformations_and_buttons() - if 'r' not in transformations: + if 'l' not in transformations or 'r' not in transformations or 'h' 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', 'oculus_head') right_controller_pose = transformations['r'] - publish_transform(right_controller_pose, 'oculus') - print('transformations', transformations) - print('buttons', buttons) + 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) + 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_') + + if len(left_joint_transformations) == 0: + publish_buttons(buttons, button_publishers) if __name__ == '__main__': main() 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 +