From 8f3d2a2a63c868ee19211a77fde08724bf632fbe Mon Sep 17 00:00:00 2001 From: eric Date: Sun, 11 Feb 2024 14:25:59 -0500 Subject: [PATCH 1/5] started cache --- src/teleoperation/frontend/src/components/SATask.vue | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index 387168430..7443cf370 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -81,6 +81,9 @@
+
+ +
@@ -102,6 +105,7 @@ import CalibrationCheckbox from './CalibrationCheckbox.vue' import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' +import Cache from './SACache.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' export default { @@ -109,6 +113,7 @@ export default { BasicMap, SoilData, BasicWaypointEditor, + Cache, Cameras, DriveControls, MastGimbalControls, From 097b1d74e9466f8feb706329632a90c873e8a46f Mon Sep 17 00:00:00 2001 From: jnnanni Date: Sun, 18 Feb 2024 14:05:31 -0500 Subject: [PATCH 2/5] Set up frontend of cache controls on ISH GUI, frontend position handling and all backend logic still needs to be looked into --- src/teleoperation/backend/consumers.py | 62 +++++++ .../frontend/src/components/CacheControls.vue | 170 ++++++++++++++++++ .../src/components/CalibrationCheckbox.vue | 1 + .../frontend/src/components/ISHTask.vue | 16 +- .../frontend/src/components/SATask.vue | 5 - 5 files changed, 246 insertions(+), 8 deletions(-) create mode 100644 src/teleoperation/frontend/src/components/CacheControls.vue diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index bb3963d7a..a609b33bf 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -69,6 +69,9 @@ def connect(self): self.sa_throttle_cmd_pub = rospy.Publisher("sa_throttle_cmd", Throttle, queue_size=1) self.sa_velocity_cmd_pub = rospy.Publisher("sa_velocity_cmd", Velocity, queue_size=1) self.sa_position_cmd_pub = rospy.Publisher("sa_position_cmd", Position, queue_size=1) + self.cache_throttle_cmd_pub = rospy.Publisher("cache_throttle_cmd", Throttle, queue_size=1) + self.cache_velocity_cmd_pub = rospy.Publisher("cache_velocity_cmd", Velocity, queue_size=1) + self.cache_position_cmd_pub = rospy.Publisher("cache_position_cmd", Position, queue_size=1) # Subscribers self.pdb_sub = rospy.Subscriber("/pdb_data", PDLB, self.pdb_callback) @@ -209,6 +212,65 @@ def to_velocity(self, input: int, joint_name: str, brushless: bool = True) -> fl / 2 ) + def hand_cache_message(self, msg): + CACHE = ["cache"] + raw_left_trigger = msg["axes"][self.xbox_mappings["left_trigger"]] + left_trigger = raw_left_trigger if raw_left_trigger > 0 else 0 + raw_right_trigger = msg["axes"][self.xbox_mappings["right_trigger"]] + right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 + if msg["arm_mode"] == "position": + sa_position_cmd = Position( + names=SA_NAMES, + positions=msg["positions"], + ) + self.sa_position_cmd_pub.publish(sa_position_cmd) + + elif msg["arm_mode"] == "velocity": + sa_velocity_cmd = Velocity() + sa_velocity_cmd.names = SA_NAMES + sa_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", False + ), + 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"), + ] + + self.sa_velocity_cmd_pub.publish(sa_velocity_cmd) + + elif msg["arm_mode"] == "throttle": + sa_throttle_cmd = Throttle() + sa_throttle_cmd.names = SA_NAMES + + sa_throttle_cmd.throttles = [ + self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) + for name, info in self.sa_config.items() + if name.startswith("sa") + ] + sa_throttle_cmd.throttles.extend( + [ + 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 sa_throttle_cmd.throttles[i] > 0): + sa_throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] + + self.sa_throttle_cmd_pub.publish(sa_throttle_cmd) + def handle_sa_arm_message(self, msg): SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] raw_left_trigger = msg["axes"][self.xbox_mappings["left_trigger"]] diff --git a/src/teleoperation/frontend/src/components/CacheControls.vue b/src/teleoperation/frontend/src/components/CacheControls.vue new file mode 100644 index 000000000..9141d24cd --- /dev/null +++ b/src/teleoperation/frontend/src/components/CacheControls.vue @@ -0,0 +1,170 @@ + + + + + diff --git a/src/teleoperation/frontend/src/components/CalibrationCheckbox.vue b/src/teleoperation/frontend/src/components/CalibrationCheckbox.vue index eb8c4ad63..c89339dae 100644 --- a/src/teleoperation/frontend/src/components/CalibrationCheckbox.vue +++ b/src/teleoperation/frontend/src/components/CalibrationCheckbox.vue @@ -100,6 +100,7 @@ export default defineComponent({ padding: 1% 0 1% 0; display: flex; flex-direction: row; + /* width: 100%; */ } .led { diff --git a/src/teleoperation/frontend/src/components/ISHTask.vue b/src/teleoperation/frontend/src/components/ISHTask.vue index 2b17cec9f..ce923a3b8 100644 --- a/src/teleoperation/frontend/src/components/ISHTask.vue +++ b/src/teleoperation/frontend/src/components/ISHTask.vue @@ -40,6 +40,10 @@ +
+

Cache Controls

+ +
@@ -53,6 +57,7 @@ import Cameras from '../components/Cameras.vue' // import CommReadout from "./CommReadout.vue"; // import MCUReset from "./MCUReset.vue" import { disableAutonLED } from '../utils.js' +import CacheControls from './CacheControls.vue' type StringIntDictionary = { [key: string]: number @@ -65,7 +70,8 @@ export default { // Cache, // Chlorophyll, // Amino, - Cameras + Cameras, + CacheControls, // CommReadout, // MCUReset, }, @@ -131,8 +137,8 @@ export default { 'cameras siteSelect' 'cameras raman' 'cameras chlorophyll' - 'carousel chlorophyll' - 'carousel amino' + 'cache-controls chlorophyll' + 'cache-controls amino' 'cache amino'; font-family: sans-serif; height: auto; @@ -195,6 +201,10 @@ export default { grid-area: cameras; } +.cache-controls{ + grid-area: cache-controls; +} + .siteSelect { grid-area: siteSelect; } diff --git a/src/teleoperation/frontend/src/components/SATask.vue b/src/teleoperation/frontend/src/components/SATask.vue index b601ca08e..1088d8251 100644 --- a/src/teleoperation/frontend/src/components/SATask.vue +++ b/src/teleoperation/frontend/src/components/SATask.vue @@ -92,9 +92,6 @@
-
- -
@@ -116,7 +113,6 @@ import CalibrationCheckbox from './CalibrationCheckbox.vue' import MotorAdjust from './MotorAdjust.vue' import OdometryReading from './OdometryReading.vue' import SAArmControls from './SAArmControls.vue' -import Cache from './SACache.vue' import { disableAutonLED, quaternionToMapAngle } from '../utils.js' export default { @@ -124,7 +120,6 @@ export default { BasicMap, SoilData, BasicWaypointEditor, - Cache, Cameras, DriveControls, MastGimbalControls, From e0b1ef0debfffba3dcdbae69cb9479830255298d Mon Sep 17 00:00:00 2001 From: eric Date: Tue, 12 Mar 2024 15:55:52 -0400 Subject: [PATCH 3/5] update consumers.py for cache controls --- config/teleop.yaml | 3 ++ src/teleoperation/backend/consumers.py | 55 +++++++------------------- 2 files changed, 17 insertions(+), 41 deletions(-) diff --git a/config/teleop.yaml b/config/teleop.yaml index c7e69fce0..2ff4c57fd 100644 --- a/config/teleop.yaml +++ b/config/teleop.yaml @@ -56,6 +56,9 @@ teleop: sensor_actuator: multiplier: 1 slow_mode_multiplier: 0.5 + cache: + multiplier: 1 + xbox_index: 4 drive_controls: forward_back: diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index a609b33bf..80bbdc695 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -219,57 +219,30 @@ def hand_cache_message(self, msg): raw_right_trigger = msg["axes"][self.xbox_mappings["right_trigger"]] right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 if msg["arm_mode"] == "position": - sa_position_cmd = Position( - names=SA_NAMES, + cache_position_cmd = Position( + names=CACHE, positions=msg["positions"], ) - self.sa_position_cmd_pub.publish(sa_position_cmd) + self.cache_position_cmd_pub.publish(cache_position_cmd) elif msg["arm_mode"] == "velocity": - sa_velocity_cmd = Velocity() - sa_velocity_cmd.names = SA_NAMES - sa_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", False - ), - 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"), + cache_velocity_cmd = Velocity() + cache_velocity_cmd.names = CACHE + cache_velocity_cmd.velocities = [ + self.to_velocity( self.sa_config["cache"]["multiplier"] *self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper")) + ] - self.sa_velocity_cmd_pub.publish(sa_velocity_cmd) + self.cache_velocity_cmd_pub.publish(cache_velocity_cmd) elif msg["arm_mode"] == "throttle": - sa_throttle_cmd = Throttle() - sa_throttle_cmd.names = SA_NAMES + cache_throttle_cmd = Throttle() + cache_throttle_cmd.names = CACHE - sa_throttle_cmd.throttles = [ - self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) - for name, info in self.sa_config.items() - if name.startswith("sa") + cache_throttle_cmd.throttles = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") ] - sa_throttle_cmd.throttles.extend( - [ - 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 sa_throttle_cmd.throttles[i] > 0): - sa_throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] - - self.sa_throttle_cmd_pub.publish(sa_throttle_cmd) + self.cache_throttle_cmd_pub.publish(cache_throttle_cmd) def handle_sa_arm_message(self, msg): SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] From a4d81db4973a32141056f6eafd0dedb6c12b39d4 Mon Sep 17 00:00:00 2001 From: eric Date: Tue, 12 Mar 2024 16:45:56 -0400 Subject: [PATCH 4/5] moved arm control sa controls and cache controls to one backend function --- src/teleoperation/backend/consumers.py | 236 ++++++++++++------------- 1 file changed, 109 insertions(+), 127 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 80bbdc695..5084c6302 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -148,9 +148,11 @@ def receive(self, text_data): elif message["type"] == "arm_adjust": self.arm_adjust(message) elif message["type"] == "arm_values": - self.handle_arm_message(message) + self.handle_controls_message(message) elif message["type"] == "sa_arm_values": - self.handle_sa_arm_message(message) + self.handle_controls_message(message) + elif message["type"] == "cache_values": + self.handle_controls_message(message) elif message["type"] == "auton_command": self.send_auton_command(message) elif message["type"] == "teleop_enabled": @@ -211,100 +213,31 @@ 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 hand_cache_message(self, msg): + + def handle_controls_message(self, msg): CACHE = ["cache"] - raw_left_trigger = msg["axes"][self.xbox_mappings["left_trigger"]] - left_trigger = raw_left_trigger if raw_left_trigger > 0 else 0 - raw_right_trigger = msg["axes"][self.xbox_mappings["right_trigger"]] - right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 - if msg["arm_mode"] == "position": - cache_position_cmd = Position( - names=CACHE, - positions=msg["positions"], - ) - self.cache_position_cmd_pub.publish(cache_position_cmd) - - elif msg["arm_mode"] == "velocity": - cache_velocity_cmd = Velocity() - cache_velocity_cmd.names = CACHE - cache_velocity_cmd.velocities = [ - self.to_velocity( self.sa_config["cache"]["multiplier"] *self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper")) - - ] - - self.cache_velocity_cmd_pub.publish(cache_velocity_cmd) - - elif msg["arm_mode"] == "throttle": - cache_throttle_cmd = Throttle() - cache_throttle_cmd.names = CACHE - - cache_throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - self.cache_throttle_cmd_pub.publish(cache_throttle_cmd) - - def handle_sa_arm_message(self, msg): SA_NAMES = ["sa_x", "sa_y", "sa_z", "sampler", "sensor_actuator"] + RA_NAMES = self.RA_NAMES + ra_slow_mode = False raw_left_trigger = msg["axes"][self.xbox_mappings["left_trigger"]] left_trigger = raw_left_trigger if raw_left_trigger > 0 else 0 raw_right_trigger = msg["axes"][self.xbox_mappings["right_trigger"]] right_trigger = raw_right_trigger if raw_right_trigger > 0 else 0 - if msg["arm_mode"] == "position": - sa_position_cmd = Position( - names=SA_NAMES, - positions=msg["positions"], - ) - self.sa_position_cmd_pub.publish(sa_position_cmd) - - elif msg["arm_mode"] == "velocity": - sa_velocity_cmd = Velocity() - sa_velocity_cmd.names = SA_NAMES - sa_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", False - ), - 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"), - ] + 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 - self.sa_velocity_cmd_pub.publish(sa_velocity_cmd) - - elif msg["arm_mode"] == "throttle": - sa_throttle_cmd = Throttle() - sa_throttle_cmd.names = SA_NAMES - - sa_throttle_cmd.throttles = [ - self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) - for name, info in self.sa_config.items() - if name.startswith("sa") - ] - sa_throttle_cmd.throttles.extend( - [ - 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 sa_throttle_cmd.throttles[i] > 0): - sa_throttle_cmd.throttles[i] *= self.sa_config[name]["slow_mode_multiplier"] - - self.sa_throttle_cmd_pub.publish(sa_throttle_cmd) - - 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") @@ -320,19 +253,44 @@ def handle_arm_message(self, msg): base_link_in_map.position[2]-=self.ik_names["z"]*left_trigger+self.ik_names["z"]*right_trigger 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) + publishers[3].publish(arm_ik_cmd) - elif msg["arm_mode"] == "position": - arm_position_cmd = Position( - names=["joint_b", "joint_c", "joint_de_pitch", "joint_de_roll"], - positions=msg["positions"], + + 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"], ) - self.arm_position_cmd_pub.publish(arm_position_cmd) + publishers[0].publish(position_cmd) elif msg["arm_mode"] == "velocity": - arm_velocity_cmd = Velocity() - arm_velocity_cmd.names = self.RA_NAMES - arm_velocity_cmd.velocities = [ + velocity_cmd = Velocity() + velocity_cmd.names = controls_names + if msg["type"] == "cache": + velocity_cmd.velocities = [ + self.to_velocity( self.sa_config["cache"]["multiplier"] *self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper")) + + ] + elif msg["type"] == "sa_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", False + ), + 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" ), @@ -351,35 +309,59 @@ def handle_arm_message(self, msg): 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"), ] - - self.arm_velocity_cmd_pub.publish(arm_velocity_cmd) - - elif msg["arm_mode"] == "throttle": - arm_throttle_cmd = Throttle() - arm_throttle_cmd.names = self.RA_NAMES - 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 - - arm_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"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), - 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: - arm_throttle_cmd.throttles[i] *= self.ra_config[name]["slow_mode_multiplier"] - if self.ra_config[name]["invert"]: - arm_throttle_cmd.throttles[i] *= -1 - - self.arm_throttle_cmd_pub.publish(arm_throttle_cmd) + publishers[1].publish(velocity_cmd) + + elif msg["arm_mode"] == "throttle": + throttle_cmd = Throttle() + throttle_cmd.names = controls_names + if msg["type"] == "cache": + throttle_cmd.throttles = [ + self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") + ] + elif msg["type"] == "arm_values": + 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"]]), + self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), + 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_values": + throttle_cmd.throttles = [ + self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) + for name, info in self.sa_config.items() + if name.startswith("sa") + ] + + throttle_cmd.throttles.extend( + [ + 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) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch From ae5bae38c7149e326fb9c5b7b015fa7c77d497c2 Mon Sep 17 00:00:00 2001 From: eric Date: Sun, 17 Mar 2024 10:13:05 -0400 Subject: [PATCH 5/5] debugging --- src/teleoperation/backend/consumers.py | 110 +++++++++--------- .../frontend/src/components/CacheControls.vue | 12 +- 2 files changed, 64 insertions(+), 58 deletions(-) diff --git a/src/teleoperation/backend/consumers.py b/src/teleoperation/backend/consumers.py index 5084c6302..31529bb40 100644 --- a/src/teleoperation/backend/consumers.py +++ b/src/teleoperation/backend/consumers.py @@ -269,12 +269,12 @@ def handle_controls_message(self, msg): elif msg["arm_mode"] == "velocity": velocity_cmd = Velocity() velocity_cmd.names = controls_names - if msg["type"] == "cache": + if msg["type"] == "cache_values": velocity_cmd.velocities = [ - self.to_velocity( self.sa_config["cache"]["multiplier"] *self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper")) - + self.sa_config["cache"]["multiplier"] *self.filter_xbox_button(msg["buttons"], + "right_bumper", "left_bumper") ] - elif msg["type"] == "sa_values": + 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 @@ -283,7 +283,7 @@ def handle_controls_message(self, msg): 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", False + 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"] @@ -308,60 +308,60 @@ def handle_controls_message(self, msg): ), 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) + ] + publishers[1].publish(velocity_cmd) - elif msg["arm_mode"] == "throttle": - throttle_cmd = Throttle() - throttle_cmd.names = controls_names - if msg["type"] == "cache": - throttle_cmd.throttles = [ - self.sa_config["cache"]["multiplier"] * self.filter_xbox_button(msg["buttons"], "right_bumper", "left_bumper") - ] - elif msg["type"] == "arm_values": - 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"]]), - self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), - 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"), - ] + 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"] == "sa_arm_values": + 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"]]), + self.filter_xbox_axis(msg["axes"][self.ra_config["joint_de_roll"]["xbox_index"]]), + 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_values": - throttle_cmd.throttles = [ - self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) - for name, info in self.sa_config.items() - if name.startswith("sa") + 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_values": + throttle_cmd.throttles = [ + self.sa_config[name]["multiplier"] * self.filter_xbox_axis(msg["axes"][info["xbox_index"]], 0.15, True) + for name, info in self.sa_config.items() + if name.startswith("sa") + ] + + throttle_cmd.throttles.extend( + [ + 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"), ] + ) - throttle_cmd.throttles.extend( - [ - 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) + 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) def handle_joystick_message(self, msg): # Tiny deadzone so we can safely e-stop with dampen switch diff --git a/src/teleoperation/frontend/src/components/CacheControls.vue b/src/teleoperation/frontend/src/components/CacheControls.vue index 9141d24cd..429e26be3 100644 --- a/src/teleoperation/frontend/src/components/CacheControls.vue +++ b/src/teleoperation/frontend/src/components/CacheControls.vue @@ -27,7 +27,7 @@
-
+
(Number(obj.value) * Math.PI) / 180 ) + + + + // this.sendMessage({ // type: 'cache_values', // axes: axes,