From 363c8cace63ed150155ad19306700f9b99fc2009 Mon Sep 17 00:00:00 2001 From: Kavi Dey Date: Sun, 4 Jul 2021 15:05:52 -0700 Subject: [PATCH] clean up three.js transformations and reduce network traffic --- operator/operator_ui_regions.js | 5 ++++- robot/ros_connect.js | 40 ++++++++------------------------- shared/sensors.js | 23 ++++++------------- 3 files changed, 20 insertions(+), 48 deletions(-) diff --git a/operator/operator_ui_regions.js b/operator/operator_ui_regions.js index 98ef6f8..2002ff5 100644 --- a/operator/operator_ui_regions.js +++ b/operator/operator_ui_regions.js @@ -317,7 +317,7 @@ function arrangeOverlays(key) { /////////////////////////////////////////////////////// -const positionOffset = new THREE.Vector3(0.05,0,0); //(0, -0.047, -0.02); +const positionOffset = new THREE.Vector3(0, 0, 0); //(0, -0.047, -0.02); function THREEinit() { // General THREE.js setup @@ -345,6 +345,7 @@ function THREEinit() { // navMode navModeObjects = {}; + // Reach overlay circle var geo = new THREE.CircleGeometry(0.52, 32) // The arm has a 52 centimeter reach (source: https://hello-robot.com/product#:~:text=range%20of%20motion%3A%2052cm) var mat = new THREE.MeshBasicMaterial({color: 'rgb(246, 179, 107)', transparent: true, opacity: 0.25}); var circle = new THREE.Mesh(geo, mat); @@ -353,9 +354,11 @@ function THREEinit() { navModeObjects.circle = circle; outlineEffect.selectObject(circle); + // Debugging cube geo = new THREE.BoxGeometry(0.05, 0.05, 0.05); mat = new THREE.MeshBasicMaterial( { color: 0x00ff00 } ); var cube = new THREE.Mesh(geo, mat); + cube.visible = false; THREEscene.add(cube); navModeObjects.cube = cube; diff --git a/robot/ros_connect.js b/robot/ros_connect.js index 51e55c5..f53d3ea 100644 --- a/robot/ros_connect.js +++ b/robot/ros_connect.js @@ -77,16 +77,6 @@ jointStateTopic.subscribe(function(message) { // float64[] velocity // float64[] effort //imageTopic.unsubscribe() - - sendData({ - type: 'sensor', - subtype: 'head', - name: 'joint_transform', - value: { - pan: getJointValue(jointState, 'joint_head_pan'), - tilt: getJointValue(jointState, 'joint_head_tilt') - } - }); }); var tfClient = new ROSLIB.TFClient({ @@ -107,9 +97,9 @@ tfClient.subscribe('link_gripper_finger_left', function(tf) { }); }); -var link_head_tilt_tf; -tfClient.subscribe('link_head_tilt', function(tf) { - link_head_tilt_tf = tf; +var camera_color_frame_tf; +tfClient.subscribe('camera_color_frame', function(tf) { + camera_color_frame_tf = tf; sendData({ type: 'sensor', subtype: 'head', @@ -133,24 +123,12 @@ function sendTfs() { }); } - if (link_head_tilt_tf) { + if (camera_color_frame_tf) { sendData({ type: 'sensor', subtype: 'head', name: 'transform', - value: link_head_tilt_tf - }); - } - - if (jointState) { - sendData({ - type: 'sensor', - subtype: 'head', - name: 'joint_transform', - value: { - pan: getJointValue(jointState, 'joint_head_pan'), - tilt: getJointValue(jointState, 'joint_head_tilt') - } + value: camera_color_frame_tf }); } } @@ -443,12 +421,12 @@ function sendIncrementalMove(jointName, jointValueInc) { function headLookAtGripper() { console.log('attempting to send headLookAtGripper command') - if (link_gripper_finger_left_tf && link_head_tilt_tf) { + if (link_gripper_finger_left_tf && camera_color_frame_tf) { var posDifference = { - x: link_gripper_finger_left_tf.translation.x - link_head_tilt_tf.translation.x, - y: link_gripper_finger_left_tf.translation.y - link_head_tilt_tf.translation.y, - z: link_gripper_finger_left_tf.translation.z - link_head_tilt_tf.translation.z + x: link_gripper_finger_left_tf.translation.x - camera_color_frame_tf.translation.x, + y: link_gripper_finger_left_tf.translation.y - camera_color_frame_tf.translation.y, + z: link_gripper_finger_left_tf.translation.z - camera_color_frame_tf.translation.z }; // Normalize posDifference diff --git a/shared/sensors.js b/shared/sensors.js index 679a7ff..f22469c 100644 --- a/shared/sensors.js +++ b/shared/sensors.js @@ -130,8 +130,6 @@ var rotation_to_target_offset = global_target_point.clone().sub(global_rotation_ var headSensors = { "transform": function (value) { - THREEcamera.position.copy(rosPostoTHREE(value.translation)); - return // Update the rotation and translation of the THREE.js camera to match the physical one var q_ros_space = new THREE.Quaternion(value.rotation.x, value.rotation.y, value.rotation.z, value.rotation.w); @@ -154,10 +152,12 @@ var headSensors = { var target_point = rotation_point.clone().add(rotated_rotation_offset_to_target_offset); //THREEcamera.position.copy(target_point); + THREEcamera.position.copy(rosPostoTHREE(value.translation)); - var e_three_space = rosEulerToTHREE(e, 'XYZ'); + var e_three_space = rosEulerToTHREE(e, 'YZX'); THREEcamera.rotation.copy(e_three_space); + /* console.log("e_ros_space", e); console.log("q_ros_space", q_ros_space); console.log("q_inverse", q_inverse); @@ -166,16 +166,7 @@ var headSensors = { console.log("rotation_point", rotation_point.clone().multiplyScalar(100)); console.log("target_point", target_point); console.log("e_three_space", e_three_space); - }, - "joint_transform": function (value) { - if (value.pan!==0 && value.tilt!==0) { - console.log("joint_pan", value.pan); - console.log("joint_tilt", value.tilt); - - THREEcamera.rotation.x = value.tilt+Math.PI/2; - THREEcamera.rotation.z = value.pan;//+(Math.PI/2); - THREEcamera.rotation.order = 'YZX'; - } + */ } } @@ -185,9 +176,9 @@ function rosPostoTHREE(p) { function rosEulerToTHREE(e, order) { return new THREE.Euler( - 0, //limitAngle(e.x-(Math.PI/2), 0, Math.PI), - e.z+(Math.PI/2),//limitAngle(e.z+(Math.PI/2), -Math.PI, Math.PI), - e.y+(Math.PI/2),//limitAngle(e.y, -Math.PI, Math.PI), + e.z+(Math.PI/2), + 0, + e.y+(Math.PI/2), order ) }