From 52166d762268bfdf9f99aa79dcd6959281d22e9c Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sat, 17 Feb 2024 22:02:07 -0500 Subject: [PATCH 01/39] fk works --- src/teleoperation/backend/consumers.py | 2 +- .../frontend/public/meshes/arm_a.fbx | 3 + .../frontend/public/meshes/arm_b.fbx | 3 + .../frontend/public/meshes/arm_c.fbx | 3 + .../frontend/public/meshes/arm_d.fbx | 3 + .../frontend/public/meshes/arm_e.fbx | 3 + .../frontend/public/meshes/bottle.fbx | 3 + .../frontend/public/meshes/ground.fbx | 3 + .../frontend/public/meshes/hammer.fbx | 3 + .../public/meshes/primitives/cube.fbx | 3 + .../public/meshes/primitives/cylinder.fbx | 3 + .../public/meshes/primitives/sphere.fbx | 3 + .../frontend/public/meshes/rock.fbx | 3 + .../frontend/public/meshes/rover_chassis.fbx | 3 + .../public/meshes/rover_left_bogie.fbx | 3 + .../public/meshes/rover_left_rocker.fbx | 3 + .../public/meshes/rover_left_wheel.fbx | 3 + .../public/meshes/rover_right_bogie.fbx | 3 + .../public/meshes/rover_right_rocker.fbx | 3 + .../public/meshes/rover_right_wheel.fbx | 3 + .../frontend/src/components/Rover3D.vue | 64 ++++++++ .../frontend/src/router/index.ts | 9 +- src/teleoperation/frontend/src/rover_three.js | 149 ++++++++++++++++++ 23 files changed, 279 insertions(+), 2 deletions(-) create mode 100644 src/teleoperation/frontend/public/meshes/arm_a.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_b.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_c.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_d.fbx create mode 100644 src/teleoperation/frontend/public/meshes/arm_e.fbx create mode 100644 src/teleoperation/frontend/public/meshes/bottle.fbx create mode 100644 src/teleoperation/frontend/public/meshes/ground.fbx create mode 100644 src/teleoperation/frontend/public/meshes/hammer.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/cube.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx create mode 100644 src/teleoperation/frontend/public/meshes/primitives/sphere.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rock.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_chassis.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx create mode 100644 src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx create mode 100644 src/teleoperation/frontend/src/components/Rover3D.vue create mode 100644 src/teleoperation/frontend/src/rover_three.js diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index bb3963d7a..b1e6e8984 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -115,7 +115,7 @@ def connect(self): self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.flight_thread = threading.Thread(target=self.flight_attitude_listener) self.flight_thread.start() - except rospy.ROSException as e: + except Exception as e: rospy.logerr(e) def disconnect(self, close_code): diff --git a/src/teleoperation/frontend/public/meshes/arm_a.fbx b/src/teleoperation/frontend/public/meshes/arm_a.fbx new file mode 100644 index 000000000..8f7c71b41 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_a.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d72481ca801642e73fea847317225514516d10667d777c0287021a90c77befba +size 869948 diff --git a/src/teleoperation/frontend/public/meshes/arm_b.fbx b/src/teleoperation/frontend/public/meshes/arm_b.fbx new file mode 100644 index 000000000..e9360724a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_b.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0578a7fe0557e7bc86e3949cc748275386e5a472d564367a00f722f0060d47c1 +size 1127196 diff --git a/src/teleoperation/frontend/public/meshes/arm_c.fbx b/src/teleoperation/frontend/public/meshes/arm_c.fbx new file mode 100644 index 000000000..5fbded9a7 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_c.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e3bad4fb02574c6faf02cac83665d7a48972640301d0ff3089d1a7200f9b81ff +size 883884 diff --git a/src/teleoperation/frontend/public/meshes/arm_d.fbx b/src/teleoperation/frontend/public/meshes/arm_d.fbx new file mode 100644 index 000000000..be192a925 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_d.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d270eeab807b552603478d239cbe1d5dc0cc374c11cfd8aff8d49b78340df9d3 +size 316844 diff --git a/src/teleoperation/frontend/public/meshes/arm_e.fbx b/src/teleoperation/frontend/public/meshes/arm_e.fbx new file mode 100644 index 000000000..5630b17d9 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/arm_e.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b42cb29f991ff0dd2752e613388cef21e9fa5030b50bb023d70f8f9f891a0bea +size 560812 diff --git a/src/teleoperation/frontend/public/meshes/bottle.fbx b/src/teleoperation/frontend/public/meshes/bottle.fbx new file mode 100644 index 000000000..25d7a8eed --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/bottle.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:df9dfe054c07005d820d7c81906136bd1bf621ebf4e4dcf3fbfb52f7e0fb4716 +size 17596 diff --git a/src/teleoperation/frontend/public/meshes/ground.fbx b/src/teleoperation/frontend/public/meshes/ground.fbx new file mode 100644 index 000000000..05b93f260 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/ground.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8323c262d0e44f0f1d128a3c68752ef95b70a9b4602de2ff06f43cef106d4b31 +size 43772 diff --git a/src/teleoperation/frontend/public/meshes/hammer.fbx b/src/teleoperation/frontend/public/meshes/hammer.fbx new file mode 100644 index 000000000..9f8b6b9e4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/hammer.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4b5e36777f78ac597bcd49076a0cf2b674d886c315b50b5d1ff75c7bcea3123a +size 16620 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cube.fbx b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx new file mode 100644 index 000000000..9326aa8ca --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cube.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3fc4c443d8ae8ca4cca8d98e6aa37a811db6fc23ce4eb169abc3314625a5f27a +size 11756 diff --git a/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx new file mode 100644 index 000000000..02941d306 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/cylinder.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4678acc34ec93779fa178d326f321b8cc67976c7e8df754c88fea29b34ee0239 +size 12460 diff --git a/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx new file mode 100644 index 000000000..07b9cab65 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/primitives/sphere.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b5eb2df5136ad610d75ba102a17a9bee8fbb0cb253a45cc6e8bad1c527559c25 +size 13948 diff --git a/src/teleoperation/frontend/public/meshes/rock.fbx b/src/teleoperation/frontend/public/meshes/rock.fbx new file mode 100644 index 000000000..0bd101bbe --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rock.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8098184699873dfc775269d78e3a2d069344d89b69fe67d38d06a3dba6f92c4d +size 18940 diff --git a/src/teleoperation/frontend/public/meshes/rover_chassis.fbx b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx new file mode 100644 index 000000000..9e4fe6a3d --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_chassis.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:497810bf42e67701d823becb224787bd04121c788a59303bfd6d485d1268fa01 +size 1966956 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx new file mode 100644 index 000000000..4d3a9e550 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ad295bd7c739fd5b8e4d05f02db3494db37253b4315c59b60e06942fb93e3a8f +size 1569308 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx new file mode 100644 index 000000000..dcfad7ae4 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:70a8a1a16c7223473ea44751b1a42d2f5e2cbfe41e312cdf1172ccfa0f8c8bcc +size 1062556 diff --git a/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx new file mode 100644 index 000000000..ab6089917 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_left_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:13129b734d971654de759255f9a6e65c2571bab60ad668a2c325f1a92b377612 +size 556396 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx new file mode 100644 index 000000000..93b721e5a --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_bogie.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:607783822c09899857fe0e556d6f8241130f3fbae96d12356ba4beb8f79bad06 +size 1567468 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx new file mode 100644 index 000000000..73f8b8f0e --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_rocker.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:f90f0f99210eea2963f2ac4a257ff8f908f6906b82dc4bd57fa61f590786da24 +size 1062780 diff --git a/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx new file mode 100644 index 000000000..bb5022926 --- /dev/null +++ b/src/teleoperation/frontend/public/meshes/rover_right_wheel.fbx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:201aaec237d10542c6a97ee8afd7b7bca7c01b6463952872e22a3369b31fa3cb +size 550204 diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue new file mode 100644 index 000000000..41eb12690 --- /dev/null +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -0,0 +1,64 @@ + + + + + \ No newline at end of file diff --git a/src/teleoperation/frontend/src/router/index.ts b/src/teleoperation/frontend/src/router/index.ts index f34e022f1..c8681c3d0 100644 --- a/src/teleoperation/frontend/src/router/index.ts +++ b/src/teleoperation/frontend/src/router/index.ts @@ -4,6 +4,7 @@ import DMESTask from '../components/DMESTask.vue' import AutonTask from '../components/AutonTask.vue' import ISHTask from '../components/ISHTask.vue' import SATask from '../components/SATask.vue' +import Rover3D from '../components/Rover3D.vue' const routes = [ { @@ -41,7 +42,13 @@ const routes = [ path: '/ISHTask', name: 'ISHTask', component: ISHTask - } + }, + { + path: "/Control", + name: "Control", + component: Rover3D, + }, + ] const router = createRouter({ history: createWebHistory(), diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js new file mode 100644 index 000000000..ca5dd8cc9 --- /dev/null +++ b/src/teleoperation/frontend/src/rover_three.js @@ -0,0 +1,149 @@ +import * as THREE from 'three'; +import { FBXLoader } from 'three/examples/jsm/loaders/FBXLoader' +import { TrackballControls } from 'three/addons/controls/TrackballControls.js'; + +export function threeSetup(containerId) { + // Get the container where the scene should be placed + const container = document.getElementById(containerId); + + const canvas = { + width: container.clientWidth, + height: container.clientHeight + } + + // THREE.Object3D.DefaultUp = new THREE.Vector3(0, 0, 1); + + const scene = new THREE.Scene(); + const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); + camera.up.set(0, 0, 1); + + const light = new THREE.AmbientLight(0x404040); // soft white light + scene.add(light); + + const renderer = new THREE.WebGLRenderer(); + renderer.setSize(canvas.width, canvas.height); + container.appendChild(renderer.domElement); // append it to your specific HTML element + + scene.background = new THREE.Color(0xcccccc); + + var controls = new TrackballControls(camera, renderer.domElement); + controls.rotateSpeed = 1.0; + controls.zoomSpeed = 1.2; + controls.panSpeed = 0.8; + controls.noZoom = false; + controls.noPan = false; + controls.staticMoving = true; + controls.dynamicDampingFactor = 0.3; + + const joints = [ + { + name: "chassis", + file: "rover_chassis.fbx", + translation: [0.164882, 0.200235, 0.051497], + rotation: [0, 0, 0], + }, + { + name: "a", + file: "arm_a.fbx", + translation: [0.034346, 0, 0.049024], + rotation: [0, 0, 0], + }, + { + name: "b", + file: "arm_b.fbx", + translation: [0.534365, 0, 0.009056], + rotation: [0, 0, 0], + }, + { + name: "c", + file: "arm_c.fbx", + translation: [19.5129, 0.264652, 0.03498], + rotation: [0, 0, 0], + }, + { + name: "d", + file: "arm_d.fbx", + translation: [0.546033, 0, 0.088594], + rotation: [0, 0, 0], + }, + { + name: "e", + file: "arm_e.fbx", + translation: [0.044886, 0, 0], + rotation: [0, 0, 0], + } + ] + + const loader = new FBXLoader(); + for (let i = 0; i < joints.length; ++i) { + loader.load('/meshes/' + joints[i].file, (fbx) => { + // Traverse the loaded scene to find meshes or objects + fbx.traverse((child)=> { + if (child.isMesh) { + scene.add(child); + child.scale.set(1, 1, 1); + child.position.set(0, 0, 0); + } + }); + }, + (xhr) => { + console.log((xhr.loaded / xhr.total) * 100 + '% loaded') + }, + (error) => { + console.log(error) + }); + } + + camera.position.x = 2; + camera.position.y = 2; + camera.position.z = 2; + camera.lookAt(new THREE.Vector3(0, 0, 0)); + + function animate() { + requestAnimationFrame(animate); + controls.update(); + renderer.render(scene, camera); + } + + animate(); + + function fk(positions) { + + let cumulativeMatrix = new THREE.Matrix4(); + + console.log(positions); + + positions.forEach((newAngle, i) => { + let mesh = scene.getObjectByName(joints[i].name); + + let localMatrix = new THREE.Matrix4(); + + // Assume rotation around Y-axis, create rotation matrix from the angle + let rotationAngle = newAngle; // Angle for this joint + if (joints[i].name == "d") { + localMatrix.makeRotationX(rotationAngle); + } + else { + localMatrix.makeRotationY(rotationAngle); + } + + let offset = new THREE.Vector3().fromArray(joints[i].translation); + + localMatrix.setPosition(offset.x, offset.y, offset.z); + + mesh.matrixAutoUpdate = false; + mesh.matrix = cumulativeMatrix.clone(); + + cumulativeMatrix.multiply(localMatrix); + }); + } + + function updateJointAngles(newJointAngles) { + fk(newJointAngles); + } + + // Return an object with the updateJointAngles() function + return { + updateJointAngles + } +}; From 3bfc63807aac75a0e2ef182c13e2082bb340551d Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 18 Feb 2024 13:59:21 -0500 Subject: [PATCH 02/39] joint a translation works. de does not --- src/teleoperation/frontend/src/rover_three.js | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 deletions(-) diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js index ca5dd8cc9..390f151e8 100644 --- a/src/teleoperation/frontend/src/rover_three.js +++ b/src/teleoperation/frontend/src/rover_three.js @@ -17,8 +17,9 @@ export function threeSetup(containerId) { const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); camera.up.set(0, 0, 1); - const light = new THREE.AmbientLight(0x404040); // soft white light - scene.add(light); + const directionalLight = new THREE.DirectionalLight(0xffffff, 0.5); + directionalLight.position.set(1, 2, 3); + scene.add(directionalLight); const renderer = new THREE.WebGLRenderer(); renderer.setSize(canvas.width, canvas.height); @@ -80,6 +81,7 @@ export function threeSetup(containerId) { // Traverse the loaded scene to find meshes or objects fbx.traverse((child)=> { if (child.isMesh) { + child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); scene.add(child); child.scale.set(1, 1, 1); child.position.set(0, 0, 0); @@ -111,16 +113,16 @@ export function threeSetup(containerId) { let cumulativeMatrix = new THREE.Matrix4(); - console.log(positions); - positions.forEach((newAngle, i) => { let mesh = scene.getObjectByName(joints[i].name); let localMatrix = new THREE.Matrix4(); - // Assume rotation around Y-axis, create rotation matrix from the angle let rotationAngle = newAngle; // Angle for this joint - if (joints[i].name == "d") { + if(joints[i].name == "chassis") { + localMatrix.makeTranslation(0,rotationAngle,0); + } + else if (joints[i].name == "d") { localMatrix.makeRotationX(rotationAngle); } else { @@ -129,7 +131,9 @@ export function threeSetup(containerId) { let offset = new THREE.Vector3().fromArray(joints[i].translation); - localMatrix.setPosition(offset.x, offset.y, offset.z); + localMatrix.setPosition( + new THREE.Vector3().setFromMatrixPosition(localMatrix).add(offset) + ); mesh.matrixAutoUpdate = false; mesh.matrix = cumulativeMatrix.clone(); From 4445a32143ef9c2dc5dfa14a1f1f30c53c391498 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 10 Mar 2024 13:39:05 -0400 Subject: [PATCH 03/39] fk works --- src/teleoperation/backend/consumers.py | 15 +++++++++ .../frontend/src/components/Rover3D.vue | 17 ++++++---- src/teleoperation/frontend/src/rover_three.js | 33 ++++++++++++++----- 3 files changed, 50 insertions(+), 15 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index b1e6e8984..cf477ac35 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -73,6 +73,7 @@ def connect(self): # Subscribers self.pdb_sub = rospy.Subscriber("/pdb_data", PDLB, self.pdb_callback) self.arm_moteus_sub = rospy.Subscriber("/arm_controller_data", ControllerState, self.arm_controller_callback) + self.arm_joint_sub = rospy.Subscriber("/arm_joint_data", JointState, self.arm_joint_callback) self.drive_moteus_sub = rospy.Subscriber( "/drive_controller_data", ControllerState, self.drive_controller_callback ) @@ -287,6 +288,14 @@ def handle_arm_message(self, msg): arm_ik_cmd = IK(pose=Pose(position=Point(*base_link_in_map.position), orientation=Quaternion(*base_link_in_map.rotation.quaternion))) self.arm_ik_pub.publish(arm_ik_cmd) + self.send(text_data=json.dumps({ + "type": "ik", + "target": { + "position": arm_ik_cmd.pose.position, + "quaternion": arm_ik_cmd.pose.orientation + } + })) + elif msg["arm_mode"] == "position": arm_position_cmd = Position( names=["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"], @@ -654,3 +663,9 @@ def flight_attitude_listener(self): self.send(text_data=json.dumps({"type": "flight_attitude", "pitch": pitch, "roll": roll})) rate.sleep() + + def arm_joint_callback(self, msg): + self.send(text_data=json.dumps({ + "type": "fk", + "positions": msg.position + })) diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue index 41eb12690..a30573232 100644 --- a/src/teleoperation/frontend/src/components/Rover3D.vue +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -3,16 +3,16 @@

Rover 3D

-
- +
+
- +
@@ -26,7 +26,7 @@ data() { return { threeScene: null, - temp_positions: ["base", "a", "b", "c", "d"], + temp_positions: ["base", "a", "b", "c", "d", "e"], positions: [] } }, @@ -41,7 +41,12 @@ watch: { message(msg) { - this.threeScene.updateJointAngles(msg.positions); + if(msg.type == "fk") { + this.threeScene.fk(msg.positions); + } + else if(msg.type == "ik") { + this.threeScene.ik(msg.target); + } } } }) diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js index 390f151e8..8bf73f8c6 100644 --- a/src/teleoperation/frontend/src/rover_three.js +++ b/src/teleoperation/frontend/src/rover_three.js @@ -11,8 +11,6 @@ export function threeSetup(containerId) { height: container.clientHeight } - // THREE.Object3D.DefaultUp = new THREE.Vector3(0, 0, 1); - const scene = new THREE.Scene(); const camera = new THREE.PerspectiveCamera(75, canvas.width / canvas.height, 0.1, 1000); camera.up.set(0, 0, 1); @@ -27,6 +25,7 @@ export function threeSetup(containerId) { scene.background = new THREE.Color(0xcccccc); + //controls to orbit around model var controls = new TrackballControls(camera, renderer.domElement); controls.rotateSpeed = 1.0; controls.zoomSpeed = 1.2; @@ -58,13 +57,13 @@ export function threeSetup(containerId) { { name: "c", file: "arm_c.fbx", - translation: [19.5129, 0.264652, 0.03498], + translation: [0.546033, 0, 0.088594], rotation: [0, 0, 0], }, { name: "d", file: "arm_d.fbx", - translation: [0.546033, 0, 0.088594], + translation: [0.044886, 0, 0], rotation: [0, 0, 0], }, { @@ -81,7 +80,10 @@ export function threeSetup(containerId) { // Traverse the loaded scene to find meshes or objects fbx.traverse((child)=> { if (child.isMesh) { - child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); + if(joints[i].name == "d" || joints[i].name == "e") { + child.material = new THREE.MeshStandardMaterial({color: 0x00ff00}); + } + else child.material = new THREE.MeshStandardMaterial({color: 0xffffff}); scene.add(child); child.scale.set(1, 1, 1); child.position.set(0, 0, 0); @@ -101,6 +103,12 @@ export function threeSetup(containerId) { camera.position.z = 2; camera.lookAt(new THREE.Vector3(0, 0, 0)); + const targetCube = new THREE.Mesh( + new THREE.BoxGeometry( 0.01, 0.01, 0.01 ), + new THREE.MeshBasicMaterial({color: 0x00ff00}) + ); + scene.add( targetCube ); + function animate() { requestAnimationFrame(animate); controls.update(); @@ -111,6 +119,8 @@ export function threeSetup(containerId) { function fk(positions) { + console.log(positions) + let cumulativeMatrix = new THREE.Matrix4(); positions.forEach((newAngle, i) => { @@ -122,13 +132,15 @@ export function threeSetup(containerId) { if(joints[i].name == "chassis") { localMatrix.makeTranslation(0,rotationAngle,0); } - else if (joints[i].name == "d") { + else if (joints[i].name == "c") { localMatrix.makeRotationX(rotationAngle); } else { localMatrix.makeRotationY(rotationAngle); } + console.log(localMatrix) + let offset = new THREE.Vector3().fromArray(joints[i].translation); localMatrix.setPosition( @@ -142,12 +154,15 @@ export function threeSetup(containerId) { }); } - function updateJointAngles(newJointAngles) { - fk(newJointAngles); + function ik(target) { + let position = new THREE.Vector3(...target.position); + let quaternion = new THREE.Quaternion(...target.quaternion); + targetCube.setPosition(position); + targetCube.setRotationFromQuaternion(quaternion); } // Return an object with the updateJointAngles() function return { - updateJointAngles + fk, ik } }; From fc979b294f5fad5f40383ef3fb3635e50f6bfc43 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 17 Mar 2024 14:11:17 -0400 Subject: [PATCH 04/39] Temp --- src/simulator/simulator.cpp | 2 ++ src/simulator/simulator.hpp | 2 ++ src/simulator/simulator.sensors.cpp | 10 ++++++++++ src/teleoperation/backend/consumers.py | 6 +++--- src/teleoperation/frontend/src/components/DMESTask.vue | 5 ++++- src/teleoperation/frontend/src/components/Rover3D.vue | 1 + src/teleoperation/frontend/src/rover_three.js | 8 ++------ 7 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/simulator/simulator.cpp b/src/simulator/simulator.cpp index f3a536b2a..822466daa 100644 --- a/src/simulator/simulator.cpp +++ b/src/simulator/simulator.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -22,6 +23,7 @@ namespace mrover { mMotorStatusPub = mNh.advertise("/drive_status", 1); mDriveControllerStatePub = mNh.advertise("/drive_controller_data", 1); mArmControllerStatePub = mNh.advertise("/arm_controller_data", 1); + mArmJointStatePub = mNh.advertise("/arm_joint_data", 1); mIkTargetPub = mNh.advertise("/arm_ik", 1); diff --git a/src/simulator/simulator.hpp b/src/simulator/simulator.hpp index e37a5aae3..ef3e39130 100644 --- a/src/simulator/simulator.hpp +++ b/src/simulator/simulator.hpp @@ -4,6 +4,7 @@ #include "glfw_pointer.hpp" #include "wgpu_objects.hpp" +#include using namespace std::literals; @@ -213,6 +214,7 @@ namespace mrover { ros::Publisher mMotorStatusPub; ros::Publisher mDriveControllerStatePub; ros::Publisher mArmControllerStatePub; + ros::Publisher mArmJointStatePub; tf2_ros::Buffer mTfBuffer; tf2_ros::TransformListener mTfListener{mTfBuffer}; diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index ec3acfbdd..95f7bd3cf 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -1,4 +1,5 @@ #include "simulator.hpp" +#include namespace mrover { @@ -243,11 +244,19 @@ namespace mrover { mDriveControllerStatePub.publish(driveControllerState); ControllerState armControllerState; + sensor_msgs::JointState armJointState; + int[6] zeros = {0,0,0,0,0,0}; + armJointState.header.stamp = ros::Time::now(); for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { armControllerState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); armControllerState.state.emplace_back("Armed"); armControllerState.error.emplace_back("None"); + armJointState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); + armJointState.position.emplace_back(rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index)); + armJointState.velocity = zeros; + armJointState.effort = zeros; + std::uint8_t limitSwitches = 0b000; if (auto limits = rover.model.getLink(linkName)->parent_joint->limits) { double joinPosition = rover.physics->getJointPos(rover.linkNameToMeta.at(linkName).index); @@ -258,6 +267,7 @@ namespace mrover { armControllerState.limit_hit.push_back(limitSwitches); } mArmControllerStatePub.publish(armControllerState); + mArmJointStatePub.publish(armJointState); } } diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index cf477ac35..a5d40b65e 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -287,12 +287,11 @@ def handle_arm_message(self, msg): arm_ik_cmd = IK(pose=Pose(position=Point(*base_link_in_map.position), orientation=Quaternion(*base_link_in_map.rotation.quaternion))) self.arm_ik_pub.publish(arm_ik_cmd) - self.send(text_data=json.dumps({ "type": "ik", "target": { - "position": arm_ik_cmd.pose.position, - "quaternion": arm_ik_cmd.pose.orientation + "position": base_link_in_map.position.tolist(), + "quaternion": base_link_in_map.rotation.quaternion.tolist() } })) @@ -665,6 +664,7 @@ def flight_attitude_listener(self): rate.sleep() def arm_joint_callback(self, msg): + rospy.logerr("HERE") self.send(text_data=json.dumps({ "type": "fk", "positions": msg.position diff --git a/src/teleoperation/frontend/src/components/DMESTask.vue b/src/teleoperation/frontend/src/components/DMESTask.vue index 57f4df3a0..b9a97e398 100644 --- a/src/teleoperation/frontend/src/components/DMESTask.vue +++ b/src/teleoperation/frontend/src/components/DMESTask.vue @@ -52,6 +52,7 @@
+ @@ -69,6 +70,7 @@ import MotorsStatusTable from './MotorsStatusTable.vue' import OdometryReading from './OdometryReading.vue' import DriveControls from './DriveControls.vue' import MastGimbalControls from './MastGimbalControls.vue' +import Rover3D from './Rover3D.vue' export default defineComponent({ components: { @@ -82,7 +84,8 @@ export default defineComponent({ MotorsStatusTable, OdometryReading, DriveControls, - MastGimbalControls + MastGimbalControls, + Rover3D }, props: { diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue index a30573232..7b08087bf 100644 --- a/src/teleoperation/frontend/src/components/Rover3D.vue +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -42,6 +42,7 @@ watch: { message(msg) { if(msg.type == "fk") { + console.log(msg) this.threeScene.fk(msg.positions); } else if(msg.type == "ik") { diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js index 8bf73f8c6..843627963 100644 --- a/src/teleoperation/frontend/src/rover_three.js +++ b/src/teleoperation/frontend/src/rover_three.js @@ -104,7 +104,7 @@ export function threeSetup(containerId) { camera.lookAt(new THREE.Vector3(0, 0, 0)); const targetCube = new THREE.Mesh( - new THREE.BoxGeometry( 0.01, 0.01, 0.01 ), + new THREE.BoxGeometry( 0.1, 0.1, 0.1 ), new THREE.MeshBasicMaterial({color: 0x00ff00}) ); scene.add( targetCube ); @@ -119,8 +119,6 @@ export function threeSetup(containerId) { function fk(positions) { - console.log(positions) - let cumulativeMatrix = new THREE.Matrix4(); positions.forEach((newAngle, i) => { @@ -139,8 +137,6 @@ export function threeSetup(containerId) { localMatrix.makeRotationY(rotationAngle); } - console.log(localMatrix) - let offset = new THREE.Vector3().fromArray(joints[i].translation); localMatrix.setPosition( @@ -157,7 +153,7 @@ export function threeSetup(containerId) { function ik(target) { let position = new THREE.Vector3(...target.position); let quaternion = new THREE.Quaternion(...target.quaternion); - targetCube.setPosition(position); + targetCube.position.set(...target.position); targetCube.setRotationFromQuaternion(quaternion); } From 8fcb4dc07e0a9630cdb6fe4b27c18852b2ea5c9d Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 17 Mar 2024 16:49:07 -0400 Subject: [PATCH 05/39] fk and ik working --- src/teleoperation/backend/consumers.py | 25 ++++++++++++------- .../frontend/src/components/Rover3D.vue | 14 +---------- src/teleoperation/frontend/src/rover_three.js | 12 ++++----- 3 files changed, 22 insertions(+), 29 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index ac2320233..bcb4254a4 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -284,7 +284,15 @@ def handle_sa_arm_message(self, msg): def handle_arm_message(self, msg): ra_slow_mode = False if msg["arm_mode"] == "ik": - base_link_in_map = SE3.from_tf_tree(self.tf_buffer, "map", "base_link") + base_link_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") + + self.send(text_data=json.dumps({ + "type": "ik", + "target": { + "position": base_link_in_map.position.tolist(), + "quaternion": base_link_in_map.rotation.quaternion.tolist() + } + })) left_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_trigger"]]) if left_trigger < 0: @@ -308,13 +316,13 @@ def handle_arm_message(self, msg): ) ) self.arm_ik_pub.publish(arm_ik_cmd) - self.send(text_data=json.dumps({ - "type": "ik", - "target": { - "position": base_link_in_map.position.tolist(), - "quaternion": base_link_in_map.rotation.quaternion.tolist() - } - })) + # self.send(text_data=json.dumps({ + # "type": "ik", + # "target": { + # "position": base_link_in_map.position.tolist(), + # "quaternion": base_link_in_map.rotation.quaternion.tolist() + # } + # })) elif msg["arm_mode"] == "position": arm_position_cmd = Position( @@ -720,7 +728,6 @@ def flight_attitude_listener(self): rate.sleep() def arm_joint_callback(self, msg): - rospy.logerr("HERE") self.send(text_data=json.dumps({ "type": "fk", "positions": msg.position diff --git a/src/teleoperation/frontend/src/components/Rover3D.vue b/src/teleoperation/frontend/src/components/Rover3D.vue index 7b08087bf..175279981 100644 --- a/src/teleoperation/frontend/src/components/Rover3D.vue +++ b/src/teleoperation/frontend/src/components/Rover3D.vue @@ -2,18 +2,6 @@

Rover 3D

- -
- - -
-
- -
@@ -33,6 +21,7 @@ mounted() { this.threeScene = threeSetup("threejs"); + // this.threeScene.fk([0,0,0,0,0]); }, computed: { @@ -42,7 +31,6 @@ watch: { message(msg) { if(msg.type == "fk") { - console.log(msg) this.threeScene.fk(msg.positions); } else if(msg.type == "ik") { diff --git a/src/teleoperation/frontend/src/rover_three.js b/src/teleoperation/frontend/src/rover_three.js index 843627963..c42038462 100644 --- a/src/teleoperation/frontend/src/rover_three.js +++ b/src/teleoperation/frontend/src/rover_three.js @@ -121,18 +121,17 @@ export function threeSetup(containerId) { let cumulativeMatrix = new THREE.Matrix4(); - positions.forEach((newAngle, i) => { + cumulativeMatrix.makeTranslation(new THREE.Vector3(0, 0, 0.439675)); //makes meshes relative to base_link + + for(let i = 0; i < joints.length; ++i){ let mesh = scene.getObjectByName(joints[i].name); let localMatrix = new THREE.Matrix4(); - let rotationAngle = newAngle; // Angle for this joint + let rotationAngle = positions[i]; // Angle for this joint if(joints[i].name == "chassis") { localMatrix.makeTranslation(0,rotationAngle,0); } - else if (joints[i].name == "c") { - localMatrix.makeRotationX(rotationAngle); - } else { localMatrix.makeRotationY(rotationAngle); } @@ -147,11 +146,10 @@ export function threeSetup(containerId) { mesh.matrix = cumulativeMatrix.clone(); cumulativeMatrix.multiply(localMatrix); - }); + } } function ik(target) { - let position = new THREE.Vector3(...target.position); let quaternion = new THREE.Quaternion(...target.quaternion); targetCube.position.set(...target.position); targetCube.setRotationFromQuaternion(quaternion); From da25ed1ee4b0cae2dc31a144f906d53992626f37 Mon Sep 17 00:00:00 2001 From: jnnanni Date: Sun, 14 Apr 2024 13:11:44 -0400 Subject: [PATCH 06/39] Fix sending arm positions --- src/teleoperation/backend/consumers.py | 18 +++---- .../frontend/src/components/ArmControls.vue | 54 +++++++++++-------- 2 files changed, 41 insertions(+), 31 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index a9c956c7b..dd93f6044 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -253,6 +253,14 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl ) def handle_controls_message(self, msg): + if msg["type"] == "arm_values" and msg["arm_mode"] == "position": + position_names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] + position_cmd = Position( + names=position_names, + positions=msg["positions"], + ) + self.arm_position_cmd_pub.publish(position_cmd) + return CACHE = ["cache"] SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] RA_NAMES = self.RA_NAMES @@ -302,16 +310,6 @@ def handle_controls_message(self, msg): ) publishers[3].publish(arm_ik_cmd) - if msg["arm_mode"] == "position": - position_names = controls_names - if msg["type"] == "arm_values": - position_names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] - position_cmd = Position( - names=position_names, - positions=msg["positions"], - ) - publishers[0].publish(position_cmd) - elif msg["arm_mode"] == "velocity": velocity_cmd = Velocity() velocity_cmd.names = controls_names diff --git a/src/teleoperation/frontend/src/components/ArmControls.vue b/src/teleoperation/frontend/src/components/ArmControls.vue index f086e00fd..5279a8953 100644 --- a/src/teleoperation/frontend/src/components/ArmControls.vue +++ b/src/teleoperation/frontend/src/components/ArmControls.vue @@ -96,7 +96,6 @@ import ToggleButton from './ToggleButton.vue' import CalibrationCheckbox from './CalibrationCheckbox.vue' import MotorAdjust from './MotorAdjust.vue' import LimitSwitch from './LimitSwitch.vue' - // In seconds const updateRate = 0.01 let interval: number | undefined @@ -137,6 +136,7 @@ export default defineComponent({ } }, positions: [], + send_positions: false // Only send after submit is clicked for the first time } }, @@ -152,11 +152,17 @@ export default defineComponent({ alert('Toggling Arm Laser failed.') } } + }, + arm_mode(newMode) { + if (newMode !== 'position') { + this.positions = [] + this.send_positions = false + } } }, - mounted: function() { - document.addEventListener('keydown', this.keyDown); + mounted: function () { + document.addEventListener('keydown', this.keyDown) }, beforeUnmount: function () { @@ -166,22 +172,26 @@ export default defineComponent({ created: function () { interval = window.setInterval(() => { - const gamepads = navigator.getGamepads() - for (let i = 0; i < 4; i++) { - const gamepad = gamepads[i] - if (gamepad) { - // Microsoft and Xbox for old Xbox 360 controllers - // X-Box for new PowerA Xbox One controllers - if ( - gamepad.id.includes('Microsoft') || - gamepad.id.includes('Xbox') || - gamepad.id.includes('X-Box') - ) { - let buttons = gamepad.buttons.map((button) => { - return button.value - }) + if (this.send_positions) { + this.publishJoystickMessage([], [], this.arm_mode, this.positions) + } else { + const gamepads = navigator.getGamepads() + for (let i = 0; i < 4; i++) { + const gamepad = gamepads[i] + if (gamepad) { + // Microsoft and Xbox for old Xbox 360 controllers + // X-Box for new PowerA Xbox One controllers + if ( + gamepad.id.includes('Microsoft') || + gamepad.id.includes('Xbox') || + gamepad.id.includes('X-Box') + ) { + let buttons = gamepad.buttons.map((button) => { + return button.value + }) - this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, this.positions) + this.publishJoystickMessage(gamepad.axes, buttons, this.arm_mode, this.positions) + } } } } @@ -221,13 +231,15 @@ export default defineComponent({ this.positions = Object.values(this.temp_positions).map( (obj) => (Number(obj.value) * Math.PI) / 180 ) + this.send_positions = true + this.publishJoystickMessage([], [], this.arm_mode, this.positions) }, - keyDown: function(event: { key: string }) { + keyDown: function (event: { key: string }) { if (event.key == ' ') { - this.arm_mode = 'arm_disabled'; + this.arm_mode = 'arm_disabled' } - }, + } } }) From 8bafb50593a3ec25e38b1461feb155ee2f55ab45 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Sun, 14 Apr 2024 13:22:59 -0400 Subject: [PATCH 07/39] Fixed bug when gamepad is plugged in --- src/teleoperation/frontend/src/components/ArmControls.vue | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/teleoperation/frontend/src/components/ArmControls.vue b/src/teleoperation/frontend/src/components/ArmControls.vue index 5279a8953..c8d7716e7 100644 --- a/src/teleoperation/frontend/src/components/ArmControls.vue +++ b/src/teleoperation/frontend/src/components/ArmControls.vue @@ -174,7 +174,7 @@ export default defineComponent({ interval = window.setInterval(() => { if (this.send_positions) { this.publishJoystickMessage([], [], this.arm_mode, this.positions) - } else { + } else if (this.arm_mode !== "position") { const gamepads = navigator.getGamepads() for (let i = 0; i < 4; i++) { const gamepad = gamepads[i] From b75e2583064b580a7d733547c0089b78ecb6f261 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Tue, 16 Apr 2024 11:35:57 -0400 Subject: [PATCH 08/39] fixed ik msg --- src/teleoperation/backend/consumers.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index f985d6d21..9f3b78bd2 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -275,7 +275,7 @@ def handle_controls_message(self, msg): if msg["arm_mode"] == "ik": ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") - + ee_in_map.position[0] += ( self.ik_names["x"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_x"]]), ) @@ -283,7 +283,7 @@ def handle_controls_message(self, msg): self.ik_names["y"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_y"]]), ) ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) - + arm_ik_cmd = IK(target=PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="base_link"), pose=Pose( From 5891cb14f23a5b84eab5e6080a506c9da8b4f02a Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Tue, 16 Apr 2024 22:20:36 -0400 Subject: [PATCH 09/39] reorganized arm function --- src/teleoperation/backend/consumers.py | 291 ++++++++++++------------- 1 file changed, 140 insertions(+), 151 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 9f3b78bd2..653ab5bda 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -250,167 +250,156 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl * (self.brushed_motors[joint_name]["max_velocity"] - self.brushed_motors[joint_name]["min_velocity"]) / 2 ) + + def publish_ik(self, axes): + left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") + + ee_in_map.position[0] += ( + self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]), + ) + ee_in_map.position[1] += ( + self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]), + ) + + ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) + + arm_ik_cmd = IK(target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(*ee_in_map.position), + orientation=Quaternion(*ee_in_map.rotation.quaternion), + ) + ) + ) + self.send(text_data=json.dumps({ + "type": "ik", + "target": { + "position": ee_in_map.position.tolist(), + "quaternion": ee_in_map.rotation.quaternion.tolist() + } + })) + self.arm_ik_pub.publish(arm_ik_cmd) + def publish_position(self, type, names, positions): + position_cmd = Position( + names=names, + positions=positions, + ) + if type == "arm_values": + self.arm_position_cmd_pub.publish(position_cmd) + elif type == "sa_arm_values": + self.sa_position_cmd_pub.publish(position_cmd) + elif type == "cache_values": + self.cache_position_cmd_pub.publish(position_cmd) + + def publish_velocity(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + velocity_cmd = Velocity() + velocity_cmd.names = names + if type == "cache_values": + velocity_cmd.velocities = [ + self.sa_config["cache"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_velocity_cmd_pub.publish(velocity_cmd) + elif type == "sa_arm_values": + velocity_cmd.velocities = [ + self.to_velocity( + self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True + ), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_velocity_cmd_pub.publish(velocity_cmd) + elif type == "arm_values": + velocity_cmd.velocities = [ + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a" + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c" + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" + ), + self.to_velocity( + self.filter_xbox_axis(axes[self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_velocity_cmd_pub.publish(velocity_cmd) + + def publish_throttle(self, type, names, axes, buttons): + left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + throttle_cmd = Throttle() + throttle_cmd.names = names + if type == "cache_values": + throttle_cmd.throttles = [ + self.sa_config["cache"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + ] + self.cache_throttle_cmd_pub.publish(throttle_cmd) + elif type == "arm_values": + throttle_cmd.throttles = [ + self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), + self.filter_xbox_axis( + axes[self.ra_config["joint_de_pitch"]["xbox_index_right"]] + - axes[self.ra_config["joint_de_pitch"]["xbox_index_left"]] + ), + self.filter_xbox_axis( + buttons[self.ra_config["joint_de_roll"]["xbox_index_right"]] + - buttons[self.ra_config["joint_de_roll"]["xbox_index_left"]] + ), + self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(buttons, "y", "a"), + self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(buttons, "b", "x"), + ] + self.arm_throttle_cmd_pub.publish(throttle_cmd) + elif type == "sa_arm_values": + throttle_cmd.throttles = [ + self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), + self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), + self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), + self.sa_config["sensor_actuator"]["multiplier"] + * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), + ] + self.sa_throttle_cmd_pub.publish(throttle_cmd) + def handle_controls_message(self, msg): - CACHE = ["cache"] - SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] - RA_NAMES = self.RA_NAMES - ra_slow_mode = False - left_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_trigger"]]) - right_trigger = self.filter_xbox_axis(msg["axes"][self.xbox_mappings["right_trigger"]]) - arm_pubs = [self.arm_position_cmd_pub, self.arm_velocity_cmd_pub, self.arm_throttle_cmd_pub, self.arm_ik_pub] - sa_pubs = [self.sa_position_cmd_pub, self.sa_velocity_cmd_pub, self.sa_throttle_cmd_pub] - cache_pubs = [self.cache_position_cmd_pub, self.cache_velocity_cmd_pub, self.cache_throttle_cmd_pub] - publishers = [] - controls_names = [] - if msg["type"] == "cache_values": - controls_names = CACHE - publishers = cache_pubs - elif msg["type"] == "arm_values": - controls_names = RA_NAMES - publishers = arm_pubs - elif msg["type"] == "sa_arm_values": - controls_names = SA_NAMES - publishers = sa_pubs + names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] + if msg["type"] == "sa_arm_values": + names = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] + elif msg["type"] == "cache_values": + names = ["cache"] if msg["arm_mode"] == "ik": - ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") - - ee_in_map.position[0] += ( - self.ik_names["x"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_x"]]), - ) - ee_in_map.position[1] += ( - self.ik_names["y"] * self.filter_xbox_axis(msg["axes"][self.xbox_mappings["left_js_y"]]), - ) - ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) - - arm_ik_cmd = IK(target=PoseStamped( - header=Header(stamp=rospy.Time.now(), frame_id="base_link"), - pose=Pose( - position=Point(*ee_in_map.position), - orientation=Quaternion(*ee_in_map.rotation.quaternion), - ) - ) - ) - self.send(text_data=json.dumps({ - "type": "ik", - "target": { - "position": ee_in_map.position.tolist(), - "quaternion": ee_in_map.rotation.quaternion.tolist() - } - })) - publishers[3].publish(arm_ik_cmd) + self.publish_ik(axes=msg["axes"]) elif msg["arm_mode"] == "position": - position_names = controls_names - if msg["type"] == "arm_values": - position_names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] - position_cmd = Position( - names=position_names, - positions=msg["positions"], - ) - publishers[0].publish(position_cmd) + self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) elif msg["arm_mode"] == "velocity": - velocity_cmd = Velocity() - velocity_cmd.names = controls_names - if msg["type"] == "cache_values": - velocity_cmd.velocities = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "sa_arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True - ), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - elif msg["type"] == "arm_values": - velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), "joint_a" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), "joint_c" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" - ), - self.to_velocity( - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), "joint_de_1" - ), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "b", "x"), - ] - publishers[1].publish(velocity_cmd) + self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) elif msg["arm_mode"] == "throttle": - throttle_cmd = Throttle() - throttle_cmd.names = controls_names - if msg["type"] == "cache_values": - throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "arm_values": - # print(msg["buttons"]) - - # d_pad_x = msg["axes"][self.xbox_mappings["d_pad_x"]] - # if d_pad_x > 0.5: - # ra_slow_mode = True - # elif d_pad_x < -0.5: - # ra_slow_mode = False - - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_a"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_b"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_c"]["xbox_index"]]), - self.filter_xbox_axis( - msg["axes"][self.ra_config["joint_de_pitch"]["xbox_index_right"]] - - msg["axes"][self.ra_config["joint_de_pitch"]["xbox_index_left"]] - ), - self.filter_xbox_axis( - msg["buttons"][self.ra_config["joint_de_roll"]["xbox_index_right"]] - - msg["buttons"][self.ra_config["joint_de_roll"]["xbox_index_left"]] - ), - self.ra_config["allen_key"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "y", "a"), - self.ra_config["gripper"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "b", "x"), - ] - - for i, name in enumerate(self.RA_NAMES): - if ra_slow_mode: - throttle_cmd.throttles[i] *= self.ra_config[name]["slow_mode_multiplier"] - if self.ra_config[name]["invert"]: - throttle_cmd.throttles[i] *= -1 - elif msg["type"] == "sa_arm_values": - throttle_cmd.throttles = [ - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_x"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_y"]["xbox_index"]]), - self.filter_xbox_axis(msg["axes"][self.sa_config["sa_z"]["xbox_index"]]), - self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), - self.sa_config["sensor_actuator"]["multiplier"] - * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper"), - ] - - fast_mode_activated = msg["buttons"][self.xbox_mappings["a"]] or msg["buttons"][self.xbox_mappings["b"]] - if not fast_mode_activated: - for i, name in enumerate(SA_NAMES): - # When going up (vel > 0) with SA joint 2, we DON'T want slow mode. - if not (name == "sa_y" and throttle_cmd.throttles[i] > 0): - throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] - publishers[2].publish(throttle_cmd) + self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch From c89e6d5b276c8743e4400a207c508838cc549954 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 13:39:03 -0400 Subject: [PATCH 10/39] Update --- launch/basestation.launch | 9 ++++++--- src/teleoperation/backend/consumers.py | 6 +++--- src/teleoperation/frontend/package.json | 1 + 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/launch/basestation.launch b/launch/basestation.launch index b5039bcbe..47f7dd691 100644 --- a/launch/basestation.launch +++ b/launch/basestation.launch @@ -4,14 +4,17 @@ + + + - - - + + + diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 653ab5bda..0cb13cdee 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -252,8 +252,8 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl ) def publish_ik(self, axes): - left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) - right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) + # left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) + # right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") ee_in_map.position[0] += ( @@ -263,7 +263,7 @@ def publish_ik(self, axes): self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]), ) - ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) + # ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) arm_ik_cmd = IK(target=PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="base_link"), diff --git a/src/teleoperation/frontend/package.json b/src/teleoperation/frontend/package.json index 8638e8b32..76d04eba6 100644 --- a/src/teleoperation/frontend/package.json +++ b/src/teleoperation/frontend/package.json @@ -20,6 +20,7 @@ "html2canvas": "^1.4.1", "leaflet": "^1.9.4", "quaternion-to-euler": "^0.5.0", + "three": "^0.163.0", "vue": "^3.3.4", "vue-flight-indicators": "^0.1.1", "vue-router": "^4.2.4", From 2af6befd842d74ab962d4df1a2351014fcc9cfb8 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 13:53:22 -0400 Subject: [PATCH 11/39] Format --- scripts/debug_arm_ik.py | 8 +-- src/simulator/simulator.sensors.cpp | 2 +- src/teleoperation/backend/consumers.py | 74 +++++++++++--------------- 3 files changed, 36 insertions(+), 48 deletions(-) diff --git a/scripts/debug_arm_ik.py b/scripts/debug_arm_ik.py index 99aa9b7a1..8fb2e95e4 100755 --- a/scripts/debug_arm_ik.py +++ b/scripts/debug_arm_ik.py @@ -13,13 +13,13 @@ rospy.sleep(1.0) pub.publish( - IK( - target = PoseStamped( + IK( + target=PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="base_link"), pose=Pose( position=Point(x=0.8, y=1.0, z=0.5), orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), - ) + ), ) ) ) @@ -31,7 +31,7 @@ def on_clicked_point(clicked_point: PointStamped): pose=Pose( position=clicked_point.point, orientation=Quaternion(x=0.0, y=0.0, z=0.0, w=1.0), - ) + ), ) ) pub.publish(ik) diff --git a/src/simulator/simulator.sensors.cpp b/src/simulator/simulator.sensors.cpp index 355b36541..1f1d1c4e5 100644 --- a/src/simulator/simulator.sensors.cpp +++ b/src/simulator/simulator.sensors.cpp @@ -245,7 +245,7 @@ namespace mrover { ControllerState armControllerState; sensor_msgs::JointState armJointState; - std::vector zeros = {0,0,0,0,0,0}; + std::vector zeros = {0, 0, 0, 0, 0, 0}; armJointState.header.stamp = ros::Time::now(); for (auto& linkName: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { armControllerState.name.emplace_back(armMsgToUrdf.backward(linkName).value()); diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 0cb13cdee..40918bf87 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -95,7 +95,9 @@ def connect(self): # Subscribers self.pdb_sub = rospy.Subscriber("/pdb_data", PDLB, self.pdb_callback) - self.arm_moteus_sub = rospy.Subscriber("/arm_controller_data", ControllerState, self.arm_controller_callback) + self.arm_moteus_sub = rospy.Subscriber( + "/arm_controller_data", ControllerState, self.arm_controller_callback + ) self.arm_joint_sub = rospy.Subscriber("/arm_joint_data", JointState, self.arm_joint_callback) self.drive_moteus_sub = rospy.Subscriber( "/drive_controller_data", ControllerState, self.drive_controller_callback @@ -250,36 +252,37 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl * (self.brushed_motors[joint_name]["max_velocity"] - self.brushed_motors[joint_name]["min_velocity"]) / 2 ) - + def publish_ik(self, axes): # left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) # right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") - ee_in_map.position[0] += ( - self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]), - ) - ee_in_map.position[1] += ( - self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]), - ) - + ee_in_map.position[0] += (self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]),) + ee_in_map.position[1] += (self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]),) + # ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) - arm_ik_cmd = IK(target=PoseStamped( + arm_ik_cmd = IK( + target=PoseStamped( header=Header(stamp=rospy.Time.now(), frame_id="base_link"), pose=Pose( position=Point(*ee_in_map.position), orientation=Quaternion(*ee_in_map.rotation.quaternion), - ) + ), + ) + ) + self.send( + text_data=json.dumps( + { + "type": "ik", + "target": { + "position": ee_in_map.position.tolist(), + "quaternion": ee_in_map.rotation.quaternion.tolist(), + }, + } ) ) - self.send(text_data=json.dumps({ - "type": "ik", - "target": { - "position": ee_in_map.position.tolist(), - "quaternion": ee_in_map.rotation.quaternion.tolist() - } - })) self.arm_ik_pub.publish(arm_ik_cmd) def publish_position(self, type, names, positions): @@ -301,21 +304,14 @@ def publish_velocity(self, type, names, axes, buttons): velocity_cmd.names = names if type == "cache_values": velocity_cmd.velocities = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") ] self.cache_velocity_cmd_pub.publish(velocity_cmd) elif type == "sa_arm_values": velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False - ), - self.to_velocity( - self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False - ), - self.to_velocity( - self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True - ), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_x"]["xbox_index"]]), "sa_x", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_y"]["xbox_index"]]), "sa_y", False), + self.to_velocity(self.filter_xbox_axis(axes[self.sa_config["sa_z"]["xbox_index"]]), "sa_z", True), self.sa_config["sampler"]["multiplier"] * (right_trigger - left_trigger), self.sa_config["sensor_actuator"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), @@ -323,15 +319,11 @@ def publish_velocity(self, type, names, axes, buttons): self.sa_velocity_cmd_pub.publish(velocity_cmd) elif type == "arm_values": velocity_cmd.velocities = [ - self.to_velocity( - self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a" - ), + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_a"]["xbox_index"]]), "joint_a"), self.to_velocity( self.filter_xbox_axis(axes[self.ra_config["joint_b"]["xbox_index"]]), "joint_b", False ), - self.to_velocity( - self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c" - ), + self.to_velocity(self.filter_xbox_axis(axes[self.ra_config["joint_c"]["xbox_index"]]), "joint_c"), self.to_velocity( self.filter_xbox_axis(axes[self.ra_config["joint_de_pitch"]["xbox_index"]]), "joint_de_0" ), @@ -350,8 +342,7 @@ def publish_throttle(self, type, names, axes, buttons): throttle_cmd.names = names if type == "cache_values": throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] - * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(buttons, "right_bumper", "left_bumper") ] self.cache_throttle_cmd_pub.publish(throttle_cmd) elif type == "arm_values": @@ -381,7 +372,7 @@ def publish_throttle(self, type, names, axes, buttons): * self.filter_xbox_button(buttons, "right_bumper", "left_bumper"), ] self.sa_throttle_cmd_pub.publish(throttle_cmd) - + def handle_controls_message(self, msg): names = ["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"] if msg["type"] == "sa_arm_values": @@ -798,12 +789,9 @@ def flight_attitude_listener(self): self.send(text_data=json.dumps({"type": "flight_attitude", "pitch": pitch, "roll": roll})) rate.sleep() - + def arm_joint_callback(self, msg): - self.send(text_data=json.dumps({ - "type": "fk", - "positions": msg.position - })) + self.send(text_data=json.dumps({"type": "fk", "positions": msg.position})) def science_spectral_callback(self, msg): self.send( From 049d49b8a78584c1229a553224cf63d9ee27130f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 14:35:23 -0400 Subject: [PATCH 12/39] Update --- src/simulator/simulator.render.cpp | 13 ++++ .../arm_controller/arm_controller.cpp | 20 +++--- src/teleoperation/arm_controller/pch.hpp | 1 + src/teleoperation/backend/consumers.py | 69 +++++++++---------- 4 files changed, 57 insertions(+), 46 deletions(-) diff --git a/src/simulator/simulator.render.cpp b/src/simulator/simulator.render.cpp index ce2bf2247..9f725cc11 100644 --- a/src/simulator/simulator.render.cpp +++ b/src/simulator/simulator.render.cpp @@ -208,6 +208,19 @@ namespace mrover { mWindow = GlfwPointer{w, h, WINDOW_NAME, nullptr, nullptr}; NODELET_INFO_STREAM(std::format("Created window of size: {}x{}", w, h)); + if (cv::Mat logo = imread(std::filesystem::path{std::source_location::current().file_name()}.parent_path() / "mrover_logo.png", cv::IMREAD_UNCHANGED); + logo.type() == CV_8UC4) { + cvtColor(logo, logo, cv::COLOR_BGRA2RGBA); + GLFWimage logoImage{ + .width = logo.cols, + .height = logo.rows, + .pixels = logo.data, + }; + glfwSetWindowIcon(mWindow.get(), 1, &logoImage); + } else { + ROS_WARN_STREAM("Failed to load logo image"); + } + if (glfwRawMouseMotionSupported()) glfwSetInputMode(mWindow.get(), GLFW_RAW_MOUSE_MOTION, GLFW_TRUE); glfwSetWindowUserPointer(mWindow.get(), this); diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index b0d7d23af..76414e499 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -16,19 +16,19 @@ namespace mrover { Position positions; positions.names = {"joint_a", "joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"}; positions.positions.resize(positions.names.size()); - SE3d target_frame_to_arm_b_static; + SE3d targetFrameToArmBaseLink; try { - target_frame_to_arm_b_static = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); - } catch (...) { - ROS_WARN_THROTTLE(1, "Failed to resolve information about target frame"); + targetFrameToArmBaseLink = SE3Conversions::fromTfTree(mTfBuffer, ik_target.target.header.frame_id, "arm_base_link"); + } catch (tf2::TransformException const& exception) { + ROS_WARN_STREAM_THROTTLE(1, std::format("Failed to get transform from {} to arm_base_link: {}", ik_target.target.header.frame_id, exception.what())); return; } - Eigen::Vector4d target{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z, 1}; - Eigen::Vector4d target_in_arm_b_static = target_frame_to_arm_b_static.transform() * target; - double x = target_in_arm_b_static.x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector - double z = target_in_arm_b_static.z(); - double y = target_in_arm_b_static.y(); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", target_frame_to_arm_b_static); + SE3d endEffectorInTarget{{ik_target.target.pose.position.x, ik_target.target.pose.position.y, ik_target.target.pose.position.z}, SO3d::Identity()}; + SE3d endEffectorInArmBaseLink = targetFrameToArmBaseLink * endEffectorInTarget; + double x = endEffectorInArmBaseLink.translation().x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector + double y = endEffectorInArmBaseLink.translation().y(); + double z = endEffectorInArmBaseLink.translation().z(); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", targetFrameToArmBaseLink); double gamma = 0; double x3 = x - LINK_DE * std::cos(gamma); diff --git a/src/teleoperation/arm_controller/pch.hpp b/src/teleoperation/arm_controller/pch.hpp index 2f68bad08..104f5832c 100644 --- a/src/teleoperation/arm_controller/pch.hpp +++ b/src/teleoperation/arm_controller/pch.hpp @@ -2,6 +2,7 @@ #include #include +#include #include diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 40918bf87..f5d3cfa34 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -1,17 +1,17 @@ import csv -from datetime import datetime import json import os -import pytz -from math import copysign -from math import pi -from tf.transformations import euler_from_quaternion import threading +from datetime import datetime +from math import copysign, pi +import pytz from channels.generic.websocket import JsonWebsocketConsumer + import rospy import tf2_ros -import cv2 +from backend.models import AutonWaypoint, BasicWaypoint +from geometry_msgs.msg import Twist, Pose, PoseStamped, Point, Quaternion # from cv_bridge import CvBridge from mrover.msg import ( @@ -33,16 +33,12 @@ HeaterData, ) from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama -from sensor_msgs.msg import JointState, NavSatFix, Temperature, RelativeHumidity, Image +from sensor_msgs.msg import JointState, NavSatFix, Temperature, RelativeHumidity from std_msgs.msg import String, Header from std_srvs.srv import SetBool, Trigger -from geometry_msgs.msg import Twist, Pose, PoseStamped, Point, Quaternion - +from tf.transformations import euler_from_quaternion from util.SE3 import SE3 -from backend.models import AutonWaypoint, BasicWaypoint - - DEFAULT_ARM_DEADZONE = 0.15 @@ -215,16 +211,18 @@ def receive(self, text_data): except Exception as e: rospy.logerr(e) + @staticmethod def filter_xbox_axis( - self, - value: int, + value: float, deadzone_threshold: float = DEFAULT_ARM_DEADZONE, quad_control: bool = False, ) -> float: - deadzoned_val = deadzone(value, deadzone_threshold) - return quadratic(deadzoned_val) if quad_control else deadzoned_val + value = deadzone(value, deadzone_threshold) + if quad_control: + value = quadratic(value) + return value - def filter_xbox_button(self, button_array: "List[int]", pos_button: str, neg_button: str) -> float: + def filter_xbox_button(self, button_array: list[float], pos_button: str, neg_button: str) -> float: """ Applies various filtering functions to an axis for controlling the arm :return: Return -1, 0, or 1 depending on what buttons are being pressed @@ -253,25 +251,13 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl / 2 ) - def publish_ik(self, axes): - # left_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["left_trigger"]]) - # right_trigger = self.filter_xbox_axis(axes[self.xbox_mappings["right_trigger"]]) - ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_e_link") + def publish_ik(self, axes: list[float], buttons: list[float]) -> None: + ee_in_map = SE3.from_tf_tree(self.tf_buffer, "base_link", "arm_d_link") - ee_in_map.position[0] += (self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]),) - ee_in_map.position[1] += (self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]),) + ee_in_map.position[0] += self.ik_names["x"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_x"]]) + ee_in_map.position[1] += self.ik_names["y"] * self.filter_xbox_axis(axes[self.xbox_mappings["left_js_y"]]) + ee_in_map.position[2] += self.ik_names["z"] * self.filter_xbox_button(buttons, "right_trigger", "left_trigger") - # ee_in_map.position[2] += self.ik_names["z"] * (left_trigger - right_trigger) - - arm_ik_cmd = IK( - target=PoseStamped( - header=Header(stamp=rospy.Time.now(), frame_id="base_link"), - pose=Pose( - position=Point(*ee_in_map.position), - orientation=Quaternion(*ee_in_map.rotation.quaternion), - ), - ) - ) self.send( text_data=json.dumps( { @@ -283,7 +269,18 @@ def publish_ik(self, axes): } ) ) - self.arm_ik_pub.publish(arm_ik_cmd) + + self.arm_ik_pub.publish( + IK( + target=PoseStamped( + header=Header(stamp=rospy.Time.now(), frame_id="base_link"), + pose=Pose( + position=Point(*ee_in_map.position), + orientation=Quaternion(*ee_in_map.rotation.quaternion), + ), + ) + ) + ) def publish_position(self, type, names, positions): position_cmd = Position( @@ -381,7 +378,7 @@ def handle_controls_message(self, msg): names = ["cache"] if msg["arm_mode"] == "ik": - self.publish_ik(axes=msg["axes"]) + self.publish_ik(msg["axes"], msg["buttons"]) elif msg["arm_mode"] == "position": self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) From b26ea44aa64031b1eb903d23413370531a9008eb Mon Sep 17 00:00:00 2001 From: qhdwight Date: Wed, 17 Apr 2024 21:06:33 -0400 Subject: [PATCH 13/39] Fix arm base link --- src/teleoperation/arm_controller/arm_controller.cpp | 2 +- urdf/rover/rover.urdf.xacro | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/arm_controller/arm_controller.cpp b/src/teleoperation/arm_controller/arm_controller.cpp index 76414e499..dbbe35771 100644 --- a/src/teleoperation/arm_controller/arm_controller.cpp +++ b/src/teleoperation/arm_controller/arm_controller.cpp @@ -28,7 +28,7 @@ namespace mrover { double x = endEffectorInArmBaseLink.translation().x() - END_EFFECTOR_LENGTH; // shift back by the length of the end effector double y = endEffectorInArmBaseLink.translation().y(); double z = endEffectorInArmBaseLink.translation().z(); - SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", targetFrameToArmBaseLink); + SE3Conversions::pushToTfTree(mTfBroadcaster, "arm_target", "arm_base_link", endEffectorInArmBaseLink); double gamma = 0; double x3 = x - LINK_DE * std::cos(gamma); diff --git a/urdf/rover/rover.urdf.xacro b/urdf/rover/rover.urdf.xacro index 3aedbbc29..f79b800bb 100644 --- a/urdf/rover/rover.urdf.xacro +++ b/urdf/rover/rover.urdf.xacro @@ -204,7 +204,7 @@ - + From 33d430cfc96116d4c9cf6e16aba1b4082b74958f Mon Sep 17 00:00:00 2001 From: qhdwight Date: Thu, 18 Apr 2024 15:37:53 -0400 Subject: [PATCH 14/39] Import --- src/teleoperation/backend/consumers.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 187e87794..d0e1747d3 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -12,6 +12,7 @@ import tf2_ros from backend.models import AutonWaypoint, BasicWaypoint from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3, PoseStamped + # from cv_bridge import CvBridge from mrover.msg import ( PDLB, @@ -32,7 +33,7 @@ HeaterData, ) from mrover.srv import EnableAuton, AdjustMotor, ChangeCameras, CapturePanorama -from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity +from sensor_msgs.msg import NavSatFix, Temperature, RelativeHumidity, JointState from std_msgs.msg import String, Header from std_srvs.srv import SetBool, Trigger from tf.transformations import euler_from_quaternion From 6e357546c282f7a9935db57894e7eb01da40eaa9 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 15:30:55 -0400 Subject: [PATCH 15/39] Avoid uneeded copy in usb camera, only replcae bits allocator if on gcc 13 --- src/esw/drive_bridge/main.cpp | 3 ++- src/perception/usb_camera/usb_camera.cpp | 7 ++++--- src/preload/bits/allocator.h | 6 +++++- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/esw/drive_bridge/main.cpp b/src/esw/drive_bridge/main.cpp index 9e836e462..afa2092ec 100644 --- a/src/esw/drive_bridge/main.cpp +++ b/src/esw/drive_bridge/main.cpp @@ -43,7 +43,8 @@ auto main(int argc, char** argv) -> int { } void moveDrive(geometry_msgs::Twist::ConstPtr const& msg) { - // See 11.5.1 in "Controls Engineering in the FIRST Robotics Competition" for the math + // See 13.3.1.4 in "Modern Robotics" for the math + // Link: https://hades.mech.northwestern.edu/images/7/7f/MR.pdf auto forward = MetersPerSecond{msg->linear.x}; auto turn = RadiansPerSecond{msg->angular.z}; // TODO(quintin) Don't ask me to explain perfectly why we need to cancel out a meters unit in the numerator diff --git a/src/perception/usb_camera/usb_camera.cpp b/src/perception/usb_camera/usb_camera.cpp index e83d6cac2..2452b638c 100644 --- a/src/perception/usb_camera/usb_camera.cpp +++ b/src/perception/usb_camera/usb_camera.cpp @@ -42,8 +42,7 @@ namespace mrover { mCamInfoPub = mNh.advertise(cameraInfoTopicName, 1); std::string captureFormat = std::format("video/x-raw,format=YUY2,width={},height={},framerate={}/1", width, height, framerate); - std::string convertFormat = "video/x-raw,format=BGRA"; - std::string gstString = std::format("v4l2src device={} ! {} ! videoconvert ! {} ! appsink", device, captureFormat, convertFormat); + std::string gstString = std::format("v4l2src device={} ! {} ! appsink", device, captureFormat); NODELET_INFO_STREAM(std::format("GStreamer string: {}", gstString)); cv::VideoCapture capture{gstString, cv::CAP_GSTREAMER}; if (!capture.isOpened()) throw std::runtime_error{"USB camera failed to open"}; @@ -57,7 +56,9 @@ namespace mrover { if (mImgPub.getNumSubscribers()) { auto imageMessage = boost::make_shared(); - fillImageMessage(frame, imageMessage); + cv::Mat bgra; + cvtColor(frame, bgra, cv::COLOR_YUV2BGRA_YUY2); + fillImageMessage(bgra, imageMessage); imageMessage->header.frame_id = "long_range_cam_frame"; imageMessage->header.stamp = ros::Time::now(); mImgPub.publish(imageMessage); diff --git a/src/preload/bits/allocator.h b/src/preload/bits/allocator.h index a58775c45..5d5595bbd 100644 --- a/src/preload/bits/allocator.h +++ b/src/preload/bits/allocator.h @@ -6,7 +6,7 @@ // C++ should have never broke this backwards compatibility! // How silly! -#if defined(__linux__) && defined(__GNUC__) +#if defined(__linux__) && defined(__GNUC__) && (__GNUC__ == 13) // Allocators -*- C++ -*- @@ -285,4 +285,8 @@ namespace std _GLIBCXX_VISIBILITY(default) { #endif +#else + +#include_next + #endif From c5174be310cabf943e712502039e5a354d543401 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 15:35:11 -0400 Subject: [PATCH 16/39] Lower speed --- config/esw.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index f0a78c30f..3c877b92a 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -122,10 +122,10 @@ brushless_motors: max_velocity: 20.0 max_torque: 5.0 joint_a: - # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. + # 1236.8475 rad/meter, or 196.850393476 rev/meter, and we want a max vel of 40 meter/s. # gear ratio is currently at 0.005080 revolutions = 1 meter - min_velocity: -0.10 # this means -0.10 meters per second. - max_velocity: 0.10 + min_velocity: -0.05 # this means -0.10 meters per second. + max_velocity: 0.05 limit_0_present: true limit_1_present: true limit_0_enabled: true @@ -144,8 +144,8 @@ brushless_motors: max_backward_pos: 0.0 max_torque: 20.0 joint_c: - min_velocity: -0.08 # in terms of output - max_velocity: 0.08 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.04 # in terms of output + max_velocity: 0.04 # max output shaft speed: 5 rpm (for now) min_position: -0.125 max_position: 0.30 # 220 degrees of motion is the entire range max_torque: 20.0 @@ -595,4 +595,4 @@ auton_led_driver: baud: 115200 science: - shutoff_temp: 50.0f \ No newline at end of file + shutoff_temp: 50.0f From ac5fe3bde0992d2fddc64aa6383e6ce3c4cf79cb Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 15:41:07 -0400 Subject: [PATCH 17/39] Fix catkin profile --- ansible/roles/build/files/profiles/ci/config.yaml | 4 ++-- ansible/roles/build/files/profiles/debug/config.yaml | 4 ++-- ansible/roles/build/files/profiles/release/config.yaml | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ansible/roles/build/files/profiles/ci/config.yaml b/ansible/roles/build/files/profiles/ci/config.yaml index d14216f40..c0346527b 100644 --- a/ansible/roles/build/files/profiles/ci/config.yaml +++ b/ansible/roles/build/files/profiles/ci/config.yaml @@ -5,9 +5,9 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DMROVER_IS_CI=ON - -Wno-dev devel_layout: linked diff --git a/ansible/roles/build/files/profiles/debug/config.yaml b/ansible/roles/build/files/profiles/debug/config.yaml index 71005790a..434ae5ae9 100644 --- a/ansible/roles/build/files/profiles/debug/config.yaml +++ b/ansible/roles/build/files/profiles/debug/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Debug - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-pipe + - -DCMAKE_CXX_FLAGS=-pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -Wno-dev devel_layout: linked devel_space: devel diff --git a/ansible/roles/build/files/profiles/release/config.yaml b/ansible/roles/build/files/profiles/release/config.yaml index f0c29e538..ef35c5cb7 100644 --- a/ansible/roles/build/files/profiles/release/config.yaml +++ b/ansible/roles/build/files/profiles/release/config.yaml @@ -5,11 +5,11 @@ catkin_make_args: [ ] cmake_args: - -DCMAKE_BUILD_TYPE=Release - -DCMAKE_C_COMPILER=clang-16 - - -DCMAKE_CXX_FLAGS=-march=native -pipe + - -DCMAKE_CXX_FLAGS=-march=native -pipe -fgnuc-version=13.1.0 - -DCMAKE_CXX_COMPILER=clang++-16 - -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - -DCMAKE_CUDA_COMPILER_LAUNCHER=ccache - - -DCMAKE_CUDA_HOST_COMPILER=clang++-16 + - -DCMAKE_CUDA_HOST_COMPILER=g++-9 - -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=ON - -Wno-dev devel_layout: linked From ddb39f451af9991afc61ea3c1cb84ef37b22a179 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 16:55:46 -0400 Subject: [PATCH 18/39] Add arm automation init --- CMakeLists.txt | 6 ++ action/ArmAction.action | 3 + config/teleop.yaml | 13 +++- setup.py | 1 + src/teleoperation/arm_automation/__init__.py | 0 .../arm_automation/arm_automation.py | 60 +++++++++++++++++++ src/teleoperation/backend/consumers.py | 29 ++++++--- 7 files changed, 102 insertions(+), 10 deletions(-) create mode 100644 action/ArmAction.action create mode 100644 src/teleoperation/arm_automation/__init__.py create mode 100755 src/teleoperation/arm_automation/arm_automation.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..b363cfdf1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,15 +67,19 @@ set(MROVER_ROS_DEPENDENCIES tf2 tf2_ros tf2_geometry_msgs + actionlib_msgs ) extract_filenames(msg/*.msg MROVER_MESSAGE_FILES) extract_filenames(srv/*.srv MROVER_SERVICE_FILES) +extract_filenames(action/*.action MROVER_ACTION_FILES) + set(MROVER_MESSAGE_DEPENDENCIES std_msgs sensor_msgs + actionlib_msgs ) set(MROVER_PARAMETERS @@ -123,6 +127,8 @@ add_message_files(FILES ${MROVER_MESSAGE_FILES}) add_service_files(FILES ${MROVER_SERVICE_FILES}) +add_action_files(DIRECTORY action FILES ${MROVER_ACTION_FILES}) + generate_messages(DEPENDENCIES ${MROVER_MESSAGE_DEPENDENCIES}) generate_dynamic_reconfigure_options(${MROVER_PARAMETERS}) diff --git a/action/ArmAction.action b/action/ArmAction.action new file mode 100644 index 000000000..9cc4f5321 --- /dev/null +++ b/action/ArmAction.action @@ -0,0 +1,3 @@ +string name +--- +--- diff --git a/config/teleop.yaml b/config/teleop.yaml index 5aafb02ee..03676355d 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -74,14 +74,23 @@ teleop: left_js_y: 1 right_js_x: 2 right_js_y: 3 - left_trigger: 6 - right_trigger: 7 a: 0 b: 1 x: 2 y: 3 left_bumper: 4 right_bumper: 5 + left_trigger: 6 + right_trigger: 7 + back: 8 + forward: 9 + left_stick_click: 10 + right_stick_click: 11 + dpad_up: 12 + dpad_down: 13 + dpad_left: 14 + dpad_right: 15 + home: 16 joystick_mappings: left_right: 0 diff --git a/setup.py b/setup.py index 48ba751d1..e5807a321 100644 --- a/setup.py +++ b/setup.py @@ -13,6 +13,7 @@ "navigation.failure_identification", "esw", "teleoperation.teleoperation", + "teleoperation.arm_controller", ], package_dir={"": "src"}, ) diff --git a/src/teleoperation/arm_automation/__init__.py b/src/teleoperation/arm_automation/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/teleoperation/arm_automation/arm_automation.py b/src/teleoperation/arm_automation/arm_automation.py new file mode 100755 index 000000000..0bd6afae6 --- /dev/null +++ b/src/teleoperation/arm_automation/arm_automation.py @@ -0,0 +1,60 @@ +#!/usr/bin/env python3 + +import rospy + +import actionlib + +from mrover.msg import ArmActionAction, ArmActionGoal, ArmActionResult, Position + +from sensor_msgs.msg import JointState + + +def arm_automation() -> None: + rospy.init_node("arm_automation") + + server = None + joint_state = None + + pos_pub = rospy.Publisher("arm_position_cmd", Position, queue_size=1) + + def joint_state_callback(msg: JointState): + nonlocal joint_state + joint_state = msg + + rospy.Subscriber("joint_states", JointState, joint_state_callback) + + def execute_callback(goal: ArmActionGoal): + success = True + + rate = rospy.Rate(20) + while not rospy.is_shutdown(): + if server.is_preempt_requested(): + server.set_preempted() + success = False + break + + if goal.name == "de_home": + pos_pub.publish(Position(names=["joint_de_pitch", "joint_de_roll"], position=[0, 0])) + if joint_state is None: + success = False + break + + if abs(joint_state.position[0]) < 0.1 and abs(joint_state.position[1]) < 0.1: + break + + rate.sleep() + + if success: + server.set_succeeded(ArmActionResult()) + else: + server.set_aborted(ArmActionResult()) + + server = actionlib.SimpleActionServer("arm_action", ArmActionAction, execute_cb=execute_callback, auto_start=False) + server.start() + + +if __name__ == "__main__": + try: + arm_automation() + except rospy.ROSInterruptException: + pass diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index fef797f8c..fdc58227f 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -13,8 +13,12 @@ from backend.models import AutonWaypoint, BasicWaypoint from geometry_msgs.msg import Twist, Pose, Point, Quaternion, Vector3, PoseStamped +import actionlib + # from cv_bridge import CvBridge from mrover.msg import ( + ArmActionAction, + ArmActionGoal, PDLB, ControllerState, GPSWaypoint, @@ -381,17 +385,26 @@ def handle_controls_message(self, msg): elif msg["type"] == "cache_values": names = ["cache"] - if msg["arm_mode"] == "ik": - self.publish_ik(msg["axes"], msg["buttons"]) + if msg["buttons"][self.xbox_mappings["home"]] > 0.5: + client = actionlib.SimpleActionClient("arm_action", ArmActionAction) + client.wait_for_server() + + goal = ArmActionGoal(name="de_home") + client.send_goal(goal) + + client.wait_for_result() + else: + if msg["arm_mode"] == "ik": + self.publish_ik(msg["axes"], msg["buttons"]) - elif msg["arm_mode"] == "position": - self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) + elif msg["arm_mode"] == "position": + self.publish_position(type=msg["type"], names=names, positions=msg["positions"]) - elif msg["arm_mode"] == "velocity": - self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) + elif msg["arm_mode"] == "velocity": + self.publish_velocity(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) - elif msg["arm_mode"] == "throttle": - self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) + elif msg["arm_mode"] == "throttle": + self.publish_throttle(type=msg["type"], names=names, axes=msg["axes"], buttons=msg["buttons"]) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch From 671f07156e44fbad1dc923bb7523a2e9947ee6d8 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 17:01:30 -0400 Subject: [PATCH 19/39] Temp fix --- CMakeLists.txt | 2 +- src/preload/bits/allocator.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index b363cfdf1..930a5dbbc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -102,7 +102,7 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) diff --git a/src/preload/bits/allocator.h b/src/preload/bits/allocator.h index 5d5595bbd..c33fb23b3 100644 --- a/src/preload/bits/allocator.h +++ b/src/preload/bits/allocator.h @@ -6,7 +6,7 @@ // C++ should have never broke this backwards compatibility! // How silly! -#if defined(__linux__) && defined(__GNUC__) && (__GNUC__ == 13) +#if defined(__linux__) && defined(__GLIBCXX__) && !defined(__CUDACC__) // Allocators -*- C++ -*- From 189b267db7c889df242d2ed786a894b030036008 Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 20 Apr 2024 17:29:43 -0400 Subject: [PATCH 20/39] Update --- .../arm_automation/arm_automation.py | 52 +++++++++++++------ 1 file changed, 36 insertions(+), 16 deletions(-) diff --git a/src/teleoperation/arm_automation/arm_automation.py b/src/teleoperation/arm_automation/arm_automation.py index 0bd6afae6..f64533273 100755 --- a/src/teleoperation/arm_automation/arm_automation.py +++ b/src/teleoperation/arm_automation/arm_automation.py @@ -8,6 +8,8 @@ from sensor_msgs.msg import JointState +import numpy as np + def arm_automation() -> None: rospy.init_node("arm_automation") @@ -21,33 +23,51 @@ def joint_state_callback(msg: JointState): nonlocal joint_state joint_state = msg - rospy.Subscriber("joint_states", JointState, joint_state_callback) + rospy.Subscriber("arm_joint_data", JointState, joint_state_callback) def execute_callback(goal: ArmActionGoal): - success = True - + if joint_state is None: + rospy.logerr("No joint state data available") + server.set_aborted(ArmActionResult()) + return + + if goal.name == "de_home": + target_names = ["joint_de_pitch", "joint_de_roll"] + target_positions = [ + joint_state.position[joint_state.name.index('joint_de_pitch')], + np.pi / 8 + ] + rospy.loginfo(f"Moving to {target_positions} for {target_names}") + else: + rospy.logerr("Invalid goal name") + server.set_aborted(ArmActionResult()) + return + rate = rospy.Rate(20) while not rospy.is_shutdown(): if server.is_preempt_requested(): server.set_preempted() - success = False - break + rospy.loginfo("Preempted") + server.set_aborted(ArmActionResult()) + return - if goal.name == "de_home": - pos_pub.publish(Position(names=["joint_de_pitch", "joint_de_roll"], position=[0, 0])) - if joint_state is None: - success = False - break - if abs(joint_state.position[0]) < 0.1 and abs(joint_state.position[1]) < 0.1: - break + pos_pub.publish(Position(names=target_names, positions=target_positions)) + + feedback = [ + joint_state.position[joint_state.name.index('joint_de_pitch')], + joint_state.position[joint_state.name.index('joint_de_roll')] + ] + + if np.allclose(target_positions, feedback, atol=0.1): + rospy.loginfo("Reached target") + break rate.sleep() - if success: - server.set_succeeded(ArmActionResult()) - else: - server.set_aborted(ArmActionResult()) + server.set_succeeded(ArmActionResult()) + + rospy.sleep(1) server = actionlib.SimpleActionServer("arm_action", ArmActionAction, execute_cb=execute_callback, auto_start=False) server.start() From e4e59a13ff725a9d3b987b80a569b00cbf162ea4 Mon Sep 17 00:00:00 2001 From: umroverPerception Date: Sat, 20 Apr 2024 18:55:37 -0400 Subject: [PATCH 21/39] Updates --- config/esw.yaml | 6 +++--- launch/esw_base.launch | 24 ++++++++++++------------ 2 files changed, 15 insertions(+), 15 deletions(-) diff --git a/config/esw.yaml b/config/esw.yaml index 3c877b92a..c68689546 100644 --- a/config/esw.yaml +++ b/config/esw.yaml @@ -144,11 +144,11 @@ brushless_motors: max_backward_pos: 0.0 max_torque: 20.0 joint_c: - min_velocity: -0.04 # in terms of output - max_velocity: 0.04 # max output shaft speed: 5 rpm (for now) + min_velocity: -0.03 # in terms of output + max_velocity: 0.03 # max output shaft speed: 5 rpm (for now) min_position: -0.125 max_position: 0.30 # 220 degrees of motion is the entire range - max_torque: 20.0 + max_torque: 200.0 joint_de_0: min_velocity: -5.0 max_velocity: 5.0 diff --git a/launch/esw_base.launch b/launch/esw_base.launch index e2ccf7d2d..e69ff7fa2 100644 --- a/launch/esw_base.launch +++ b/launch/esw_base.launch @@ -23,14 +23,6 @@ --> - - @@ -47,15 +39,23 @@ -
From 434d2e43ea5adf3d932d14c9b6a7cdd321ad8e5b Mon Sep 17 00:00:00 2001 From: qhdwight Date: Mon, 22 Apr 2024 21:50:24 -0400 Subject: [PATCH 28/39] Add micro adjust for drive --- config/teleop.yaml | 4 +++- src/teleoperation/backend/consumers.py | 6 +++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/config/teleop.yaml b/config/teleop.yaml index bcdc5940f..1b8c68952 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -57,7 +57,9 @@ teleop: enabled: true multiplier: 1 twist: - multiplier: 0.7 + multiplier: 0.6 + tilt: + multiplier: 1 xbox_mappings: left_x: 0 diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 0580d6a47..75db25f50 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -433,6 +433,8 @@ def get_axes_input( # angular_from_lateral = get_axes_input("left_right", 0.4, True) angular = get_axes_input("twist", 0.03, True, self.max_angular_speed * dampen) + linear += get_axes_input("tilt", 0.5, scale=0.1) + self.twist_pub.publish( Twist( linear=Vector3(x=linear), @@ -812,9 +814,7 @@ def flight_attitude_listener(self): def arm_joint_callback(self, msg: JointState) -> None: # Set non-finite values to zero - for i in range(len(msg.position)): - if not isfinite(msg.position[i]): - msg.position[i] = 0 + msg.position = [x if isfinite(x) else 0 for x in msg.position] self.send(text_data=json.dumps({"type": "fk", "positions": msg.position})) def science_spectral_callback(self, msg): From 251462816b478707c9e3660e13d2fe897049d4de Mon Sep 17 00:00:00 2001 From: qhdwight Date: Tue, 23 Apr 2024 00:02:32 -0400 Subject: [PATCH 29/39] Supress cuda warning --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7c68974f1..4e916101b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ if (ZED_FOUND) set(CMAKE_CUDA_STANDARD 17) set(CMAKE_CUDA_STANDARD_REQUIRED ON) set(CMAKE_CUDA_SEPARABLE_COMPILATION ON) - set(CMAKE_CUDA_FLAGS "--diag-suppress 108,68") + set(CMAKE_CUDA_FLAGS "--diag-suppress 20012") # Jetson Xavier NX/AGX has Volta 7.2 architecture # Perception Laptop (A4000, Quadro version of RTX 3080) has Ampere 8.6 architecture set(CMAKE_CUDA_ARCHITECTURES 72 86) From f2147db8225576522a1a29c3fb6ab23f7b4370fe Mon Sep 17 00:00:00 2001 From: jnnanni Date: Tue, 23 Apr 2024 19:28:52 -0400 Subject: [PATCH 30/39] SA_z encoder in inches --- src/teleoperation/frontend/src/components/SAArmControls.vue | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/teleoperation/frontend/src/components/SAArmControls.vue b/src/teleoperation/frontend/src/components/SAArmControls.vue index 3aaed73e4..5047bd2db 100644 --- a/src/teleoperation/frontend/src/components/SAArmControls.vue +++ b/src/teleoperation/frontend/src/components/SAArmControls.vue @@ -46,7 +46,7 @@
-

SA Z Position: {{ z_position }}

+

SA Z Position: {{ z_position }} inches

- + From 4c766ade3c842f9b726d9dea784bb871723660d1 Mon Sep 17 00:00:00 2001 From: nehakankanala Date: Wed, 24 Apr 2024 18:16:58 -0400 Subject: [PATCH 32/39] added gps driver back in --- src/localization/gps_driver.py | 137 +++++++++++++++++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 src/localization/gps_driver.py diff --git a/src/localization/gps_driver.py b/src/localization/gps_driver.py new file mode 100644 index 000000000..0ed6ea0c3 --- /dev/null +++ b/src/localization/gps_driver.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +""" +Reads and parses NMEA messages from the onboard GPS to provide +location data to the rover over LCM (/gps). Subscribes to +/rtcm and passes RTCM messages to the onboard gps to +acquire an RTK fix. +""" +import serial +import asyncio +import rospy +import threading +import numpy as np +from os import getenv + +import rospy + +# from rover_msgs import GPS, RTCM +from pyubx2 import UBXReader, UBX_PROTOCOL, RTCM3_PROTOCOL, protocol, UBXMessage +from std_msgs.msg import Header +from sensor_msgs.msg import NavSatFix + +# from rtcm_msgs.msg import Message +# from mrover.msg import rtkStatus +import datetime + + +class GPS_Driver: + def __init__(self): + rospy.init_node("gps_driver") + self.port = "/dev/gps" + self.baud = 115200 + # self.base_station_sub = rospy.Subscriber("/rtcm", Message, self.process_rtcm) + self.gps_pub = rospy.Publisher("fix", NavSatFix, queue_size=1) + # self.rtk_fix_pub = rospy.Publisher("rtk_fix_status", rtkStatus, queue_size=1) + + self.lock = threading.Lock() + self.valid_offset = False + self.time_offset = -1 + + def connect(self): + # open connection to rover GPS + self.ser = serial.Serial(self.port, self.baud) + self.reader = UBXReader(self.ser, protfilter=(UBX_PROTOCOL | RTCM3_PROTOCOL)) + + return self + + def exit(self) -> None: + # close connection + self.ser.close() + + # def process_rtcm(self, data) -> None: + # print("processing RTCM") + # with self.lock: + # # rtcm_data = RTCM.decode(data) + # self.ser.write(data.message) + + def parse_rover_gps_data(self, rover_gps_data) -> None: + msg = rover_gps_data + # skip if message could not be parsed + if not msg: + return + + if rover_gps_data.identity == "RXM-RTCM": + rospy.loginfo("RXM") + msg_used = msg.msgUsed + + if msg_used == 0: + rospy.logwarn("RTCM Usage unknown\n") + elif msg_used == 1: + rospy.logwarn("RTCM message not used\n") + elif msg_used == 2: + rospy.loginfo("RTCM message successfully used by receiver\n") + + # rospy.loginfo(vars(rover_gps_data)) + + if rover_gps_data.identity == "NAV-PVT": + rospy.loginfo("PVT") + parsed_latitude = msg.lat + parsed_longitude = msg.lon + parsed_altitude = msg.hMSL + time = datetime.datetime(year=msg.year, month=msg.month, day=msg.day, hour=msg.hour, second=msg.second) + time = rospy.Time(secs=time.timestamp() + (msg.nano / 1e6)) + if not self.valid_offset: + self.time_offset = rospy.Time.now() - time + self.valid_offset = True + + time = time + self.time_offset + + rospy.loginfo_throttle(3, f"{time} {rospy.Time.now()} {time-rospy.Time.now()} {self.time_offset}") + + message_header = Header(stamp=time, frame_id="base_link") + + self.gps_pub.publish( + NavSatFix( + header=message_header, + latitude=parsed_latitude, + longitude=parsed_longitude, + altitude=parsed_altitude, + ) + ) + # self.rtk_fix_pub.publish(rtkStatus(msg_used)) + + if msg.difSoln == 1: + rospy.loginfo_throttle(3, "Differential correction applied") + + # publidh to navsatstatus in navsatfix + if msg.carrSoln == 0: + rospy.logwarn_throttle(3, "No RTK") + elif msg.carrSoln == 1: + rospy.loginfo_throttle(3, "Floating RTK Fix") + elif msg.carrSoln == 2: + rospy.loginfo_throttle(3, "RTK FIX") + + if rover_gps_data.identity == "NAV-STATUS": + pass + + def gps_data_thread(self) -> None: + # TODO: add more message checks if needed + while not rospy.is_shutdown(): + with self.lock: + if self.ser.in_waiting: + raw, rover_gps_data = self.reader.read() + parsed_gps_data = self.parse_rover_gps_data(rover_gps_data) + + +def main(): + # change values + rtk_manager = GPS_Driver() + rtk_manager.connect() + # rover_gps_thread = threading.Thread(rtk_manager.gps_data_thread) + # rover_gps_thread.start() + rtk_manager.gps_data_thread() + rtk_manager.exit() + + +if __name__ == "__main__": + main() \ No newline at end of file From b6d181bfb93cb1e660b4583b69ff16d130646dbc Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Wed, 24 Apr 2024 18:33:47 -0400 Subject: [PATCH 33/39] added map stuff --- .../frontend/src/components/DMESTask.vue | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/teleoperation/frontend/src/components/DMESTask.vue b/src/teleoperation/frontend/src/components/DMESTask.vue index c0dae73e4..688f7c916 100644 --- a/src/teleoperation/frontend/src/components/DMESTask.vue +++ b/src/teleoperation/frontend/src/components/DMESTask.vue @@ -70,6 +70,7 @@ import OdometryReading from './OdometryReading.vue' import DriveControls from './DriveControls.vue' import MastGimbalControls from './MastGimbalControls.vue' import type ArmMoteusStateTableVue from './ArmMoteusStateTable.vue' +import { quaternionToMapAngle } from '../utils.js' export default defineComponent({ components: { @@ -140,7 +141,16 @@ export default defineComponent({ this.moteusDrive.state = msg.state this.moteusDrive.error = msg.error this.moteusDrive.limit_hit = msg.limit_hit - } else if (msg.type == 'center_map') { + } + else if (msg.type == 'nav_sat_fix') { + this.odom.latitude_deg = msg.latitude + this.odom.longitude_deg = msg.longitude + this.odom.altitude = msg.altitude + } + else if (msg.type == 'auton_tfclient') { + this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) + } + else if (msg.type == 'center_map') { this.odom.latitude_deg = msg.latitude this.odom.longitude_deg = msg.longitude } From 0710cfae809aeabeabafe032e2a407dbfd760ce1 Mon Sep 17 00:00:00 2001 From: neuenfeldttj Date: Wed, 24 Apr 2024 18:51:33 -0400 Subject: [PATCH 34/39] fixed bearing --- src/teleoperation/backend/consumers.py | 8 +++---- .../frontend/src/components/AutonTask.vue | 4 ++-- .../frontend/src/components/DMESTask.vue | 8 +++++-- .../frontend/src/components/SATask.vue | 22 +++++++++++++++++-- 4 files changed, 32 insertions(+), 10 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 75db25f50..f44425176 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -187,8 +187,8 @@ def receive(self, text_data): self.send_auton_command(message) elif message["type"] == "teleop_enabled": self.send_teleop_enabled(message) - elif message["type"] == "auton_tfclient": - self.auton_bearing() + elif message["type"] == "bearing": + self.bearing() elif message["type"] == "mast_gimbal": self.mast_gimbal(message) elif message["type"] == "max_streams": @@ -655,12 +655,12 @@ def ish_thermistor_data_callback(self, msg): temps = [x.temperature for x in msg.temps] self.send(text_data=json.dumps({"type": "thermistor", "temps": temps})) - def auton_bearing(self): + def bearing(self): base_link_in_map = SE3.from_tf_tree(self.tf_buffer, "map", "base_link") self.send( text_data=json.dumps( { - "type": "auton_tfclient", + "type": "bearing", "rotation": base_link_in_map.rotation.quaternion.tolist(), } ) diff --git a/src/teleoperation/frontend/src/components/AutonTask.vue b/src/teleoperation/frontend/src/components/AutonTask.vue index 1cd400907..bf06605f8 100644 --- a/src/teleoperation/frontend/src/components/AutonTask.vue +++ b/src/teleoperation/frontend/src/components/AutonTask.vue @@ -156,7 +156,7 @@ export default defineComponent({ this.odom.latitude_deg = msg.latitude this.odom.longitude_deg = msg.longitude this.odom.altitude = msg.altitude - } else if (msg.type == 'auton_tfclient') { + } else if (msg.type == 'bearing') { this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) } else if (msg.type == "center_map") { this.odom.latitude_deg = msg.latitude @@ -179,7 +179,7 @@ export default defineComponent({ this.sendMessage({ "type": "center_map" }); }, 250) interval = setInterval(() => { - this.sendMessage({ type: 'auton_tfclient' }) + this.sendMessage({ type: 'bearing' }) }, 1000) }, diff --git a/src/teleoperation/frontend/src/components/DMESTask.vue b/src/teleoperation/frontend/src/components/DMESTask.vue index 688f7c916..f33cd09aa 100644 --- a/src/teleoperation/frontend/src/components/DMESTask.vue +++ b/src/teleoperation/frontend/src/components/DMESTask.vue @@ -69,9 +69,10 @@ import MotorsStatusTable from './MotorsStatusTable.vue' import OdometryReading from './OdometryReading.vue' import DriveControls from './DriveControls.vue' import MastGimbalControls from './MastGimbalControls.vue' -import type ArmMoteusStateTableVue from './ArmMoteusStateTable.vue' import { quaternionToMapAngle } from '../utils.js' +let interval: number + export default defineComponent({ components: { PDBFuse, @@ -147,7 +148,7 @@ export default defineComponent({ this.odom.longitude_deg = msg.longitude this.odom.altitude = msg.altitude } - else if (msg.type == 'auton_tfclient') { + else if (msg.type == 'bearing') { this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) } else if (msg.type == 'center_map') { @@ -173,6 +174,9 @@ export default defineComponent({ this.cancelIK(event) } }) + interval = setInterval(() => { + this.sendMessage({ type: 'bearing' }) + }, 1000) } }) diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 7b0bc13d0..4382b7c56 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -106,7 +106,9 @@ import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' -import { mapState } from 'vuex' +import { mapState, mapActions } from 'vuex' + +let interval: number export default { components: { @@ -175,10 +177,26 @@ export default { this.moteusState.error = msg.error this.moteusState.limit_hit = msg.limit_hit } + else if (msg.type == 'nav_sat_fix') { + this.odom.latitude_deg = msg.latitude + this.odom.longitude_deg = msg.longitude + this.odom.altitude = msg.altitude + } + else if (msg.type == 'bearing') { + this.odom.bearing_deg = quaternionToMapAngle(msg.rotation) + } } }, - created: function () {} + methods: { + ...mapActions('websocket', ['sendMessage']), + }, + + created: function () { + interval = setInterval(() => { + this.sendMessage({ type: 'bearing' }) + }, 1000) + } } From 98975cff4404fd996ec355561768aa1ce16412de Mon Sep 17 00:00:00 2001 From: qhdwight Date: Sat, 27 Apr 2024 13:29:41 -0400 Subject: [PATCH 35/39] Refactor streaming --- ansible/roles/build/tasks/main.yml | 1 + .../gst_websocket_streamer.cpp | 125 +++++++++--------- .../gst_websocket_streamer.hpp | 15 +++ .../frontend/src/components/CameraFeed.vue | 77 ++++++----- 4 files changed, 127 insertions(+), 91 deletions(-) diff --git a/ansible/roles/build/tasks/main.yml b/ansible/roles/build/tasks/main.yml index 6d91a9651..c2c6e022f 100644 --- a/ansible/roles/build/tasks/main.yml +++ b/ansible/roles/build/tasks/main.yml @@ -97,6 +97,7 @@ - libgstreamer1.0-dev - libgstreamer-plugins-base1.0-dev - vainfo + - x264 - name: Install Local APT Packages become: True diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp index a1430549b..dd7af2f46 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.cpp @@ -1,11 +1,5 @@ #include "gst_websocket_streamer.hpp" -#if defined(MROVER_IS_JETSON) -constexpr bool IS_JETSON = true; -#else -constexpr bool IS_JETSON = false; -#endif - namespace mrover { template @@ -26,7 +20,12 @@ namespace mrover { GstBuffer* buffer = gst_sample_get_buffer(sample); GstMapInfo map; gst_buffer_map(buffer, &map, GST_MAP_READ); - mStreamServer->broadcast({reinterpret_cast(map.data), map.size}); + + std::vector chunk(sizeof(ChunkHeader) + map.size); + std::memcpy(chunk.data(), &mChunkHeader, sizeof(ChunkHeader)); + std::memcpy(chunk.data() + sizeof(ChunkHeader), map.data, map.size); + + mStreamServer->broadcast(chunk); gst_buffer_unmap(buffer, &map); gst_sample_unref(sample); @@ -38,65 +37,63 @@ namespace mrover { mMainLoop = gstCheck(g_main_loop_new(nullptr, FALSE)); + // Source std::string launch; if (mCaptureDevice.empty()) { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - // App source is pushed to when we get a ROS BGRA image message - // is-live prevents frames from being pushed when the pipeline is in READY - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! videoconvert " // Convert BGRA => I420 (YUV) for the encoder, note we are still on the CPU - "! video/x-raw,format=I420 " - "! nvvidconv " // Upload to GPU memory for the encoder - "! video/x-raw(memory:NVMM),format=I420 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server - // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight, mBitrate); - } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "appsrc name=imageSource is-live=true " - "! video/x-raw,format=BGRA,width={},height={},framerate=30/1 " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mImageWidth, mImageHeight); - } + // App source is pushed to when we get a ROS BGRA image message + // is-live prevents frames from being pushed when the pipeline is in READY + launch += "appsrc name=imageSource is-live=true "; } else { - if constexpr (IS_JETSON) { - // ReSharper disable once CppDFAUnreachableCode - - // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 - // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) - - launch = std::format( + launch += std::format("v4l2src device={} ", mCaptureDevice); + } + // Source format + std::string captureFormat = mCaptureDevice.empty() ? "video/x-raw,format=BGRA" : mDecodeJpegFromDevice ? "image/jpeg" + : "video/x-raw,format=YUY2"; + launch += std::format("! {},width={},height={},framerate={}/1 ", + captureFormat, mImageWidth, mImageHeight, mImageFramerate); + // Source decoder and H265 encoder + if (gst_element_factory_find("nvv4l2h265enc")) { + // Most likely on the Jetson + if (captureFormat.contains("jpeg")) { + // Mostly used with USB cameras, MPEG capture uses way less USB bandwidth + launch += + // TODO(quintin): I had to apply this patch: https://forums.developer.nvidia.com/t/macrosilicon-usb/157777/4 + // nvv4l2camerasrc only supports UYUV by default, but our cameras are YUY2 (YUYV) // "nvv4l2camerasrc device={} " - - "v4l2src device={} " - // "! video/x-raw(memory:NVMM),format=YUY2,width={},height={},framerate={}/1 " - "! image/jpeg,width={},height={},framerate={}/1 " - "! nvv4l2decoder mjpeg=1 " - - "! nvvidconv " - "! video/x-raw(memory:NVMM),format=NV12 " - "! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight, mImageFramerate, mBitrate); + + "! nvv4l2decoder mjpeg=1 " // Hardware-accelerated JPEG decoding, output is apparently some unknown proprietary format + "! nvvidconv " // Convert from proprietary format to NV12 so the encoder understands it + "! video/x-raw(memory:NVMM),format=NV12 "; } else { - // ReSharper disable once CppDFAUnreachableCode - launch = std::format( - "v4l2src device={} " - "! video/x-raw,format=YUY2,width={},height={},framerate=30/1 " - "! videoconvert " - "! nvh265enc name=encoder " - "! appsink name=streamSink sync=false", - mCaptureDevice, mImageWidth, mImageHeight); + launch += "! videoconvert " // Convert to I420 for the encoder, note we are still on the CPU + "! video/x-raw,format=I420 " + "! nvvidconv " // Upload to GPU memory for the encoder + "! video/x-raw(memory:NVMM),format=I420 "; } + launch += std::format("! nvv4l2h265enc name=encoder bitrate={} iframeinterval=300 vbv-size=33333 insert-sps-pps=true control-rate=constant_bitrate profile=Main num-B-Frames=0 ratecontrol-enable=true preset-level=UltraFastPreset EnableTwopassCBR=false maxperf-enable=true ", + mBitrate); + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else if (gst_element_factory_find("nvh265enc")) { + // For desktop/laptops with the custom NVIDIA bad gstreamer plugins built (a massive pain to do!) + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else if (captureFormat.contains("YUY2")) + launch += "! videoconvert "; + launch += "! nvh265enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H265; + } else { + // For desktop/laptops with no hardware encoder + if (captureFormat.contains("jpeg")) + launch += "! jpegdec "; + else + launch += "! videoconvert "; + launch += "! x264enc name=encoder "; + mChunkHeader.codec = ChunkHeader::Codec::H264; } + // App sink is pulled from (getting H265 chunks) on another thread and sent to the stream server + // sync=false is needed to avoid weirdness, going from playing => ready => playing will not work otherwise + launch += "! appsink name=streamSink sync=false"; ROS_INFO_STREAM(std::format("GStreamer launch: {}", launch)); mPipeline = gstCheck(gst_parse_launch(launch.c_str(), nullptr)); @@ -145,12 +142,20 @@ namespace mrover { } } + auto widthAndHeightToResolution(std::uint32_t width, std::uint32_t height) -> ChunkHeader::Resolution { + if (width == 640 && height == 480) return ChunkHeader::Resolution::EGA; + if (width == 1280 && height == 720) return ChunkHeader::Resolution::HD; + if (width == 1920 && height == 1080) return ChunkHeader::Resolution::FHD; + throw std::runtime_error{"Unsupported resolution"}; + } + auto GstWebsocketStreamerNodelet::onInit() -> void { try { mNh = getMTNodeHandle(); mPnh = getMTPrivateNodeHandle(); mCaptureDevice = mPnh.param("device", ""); + mDecodeJpegFromDevice = mPnh.param("decode_jpeg_from_device", true); mImageTopic = mPnh.param("image_topic", "image"); mImageWidth = mPnh.param("width", 640); mImageHeight = mPnh.param("height", 480); @@ -159,6 +164,8 @@ namespace mrover { auto port = mPnh.param("port", 8081); mBitrate = mPnh.param("bitrate", 2000000); + mChunkHeader.resolution = widthAndHeightToResolution(mImageWidth, mImageHeight); + gst_init(nullptr, nullptr); initPipeline(); @@ -198,7 +205,7 @@ namespace mrover { } } catch (std::exception const& e) { - ROS_ERROR_STREAM(std::format("Exception initializing NVIDIA GST H265 Encoder: {}", e.what())); + ROS_ERROR_STREAM(std::format("Exception initializing gstreamer websocket streamer: {}", e.what())); ros::requestShutdown(); } } diff --git a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp index c4950f62c..7682a8a54 100644 --- a/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp +++ b/src/esw/gst_websocket_streamer/gst_websocket_streamer.hpp @@ -4,11 +4,24 @@ namespace mrover { + struct ChunkHeader { + enum struct Resolution : std::uint8_t { + EGA, + HD, + FHD, + } resolution; + enum struct Codec : std::uint8_t { + H265, + H264, + } codec; + }; + class GstWebsocketStreamerNodelet final : public nodelet::Nodelet { ros::NodeHandle mNh, mPnh; std::string mCaptureDevice; + bool mDecodeJpegFromDevice{}; std::string mImageTopic; std::uint64_t mBitrate{}; std::uint32_t mImageWidth{}, mImageHeight{}, mImageFramerate{}; @@ -22,6 +35,8 @@ namespace mrover { std::thread mMainLoopThread; std::thread mStreamSinkThread; + ChunkHeader mChunkHeader{}; + auto onInit() -> void override; auto pullStreamSamplesLoop() -> void; diff --git a/src/teleoperation/frontend/src/components/CameraFeed.vue b/src/teleoperation/frontend/src/components/CameraFeed.vue index efbce30af..698838b96 100644 --- a/src/teleoperation/frontend/src/components/CameraFeed.vue +++ b/src/teleoperation/frontend/src/components/CameraFeed.vue @@ -4,26 +4,26 @@

{{ name }} • ID: {{ id }}

- - - - -
- + > + + +
+