From 137b4265a99ea3286c95789eda9f58e78bb5639e Mon Sep 17 00:00:00 2001 From: Nils Bore Date: Tue, 21 Jan 2020 09:53:02 +0100 Subject: [PATCH] Added RPM and DVL feedback --- src/flexxros/sam_widgets.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/src/flexxros/sam_widgets.py b/src/flexxros/sam_widgets.py index c9aab28..3a5cb4d 100644 --- a/src/flexxros/sam_widgets.py +++ b/src/flexxros/sam_widgets.py @@ -167,15 +167,30 @@ def init(self): # We subscribe to these topics at full frquency (no extra arg) self.subscribe("/sam/core/gps", "sensor_msgs/NavSatFix", self.gps_callback) self.subscribe("/sam/core/battery_fb", "sensor_msgs/BatteryState", self.battery_callback) + self.subscribe("/sam/core/dvl_enabled", "std_msgs/Bool", self.dvl_enabled_callback) + self.subscribe("/sam/core/dvl", "cola2_msgs/DVL", self.dvl_callback) + # We only subscribe to these topics at 1hz self.subscribe("/sam/dr/odom", "nav_msgs/Odometry", self.odom_callback, 1.) self.subscribe("/sam/core/vbs_fb", "sam_msgs/PercentStamped", self.vbs_callback, 1.) self.subscribe("/sam/core/lcg_fb", "sam_msgs/PercentStamped", self.lcg_callback, 1.) + self.subscribe("/sam/core/rpm_fb", "sam_msgs/ThrusterRPMs", self.rpm_callback, 1.) self.subscribe("/sam/ctrl/depth_feedback", "std_msgs/Float64", self.depth_callback, 1.) self.subscribe("/sam/ctrl/pitch_feedback", "std_msgs/Float64", self.pitch_callback, 1.) self.subscribe("/sam/ctrl/roll_feedback", "std_msgs/Float64", self.roll_callback, 1.) self.subscribe("/sam/ctrl/yaw_feedback", "std_msgs/Float64", self.yaw_callback, 1.) + def dvl_enabled_callback(self, msg): + + if msg.data: + self.dvl_status.apply_style("background: #bbffbb;") + else: + self.dvl_status.apply_style("background: #ffb3af;") + + def dvl_callback(self, msg): + + self.dvl_status.set_text("X vel: %.02f, Alt: %.02f" % (msg.velocity.x, msg.altitude)) + def odom_callback(self, msg): self.xpos.set_text("%.02fm" % msg.pose.pose.position.x) @@ -192,6 +207,10 @@ def lcg_callback(self, msg): self.lcg_fb.set_text("%.02f%" % msg.value) + def rpm_callback(self, msg): + + self.rpm_fb.set_text("%drpm, %drpm" % (msg.thruster_1_rpm, msg.thruster_2_rpm)) + def depth_callback(self, msg): self.depth.set_text("%.02fm" % msg.data)