Added script to follow coordinates from another gpsd

[ros_wild_thumper.git] / scripts / wt_node.py

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 8589156bcf517d75aabea58ac16f5e33f62d3ec4..4a6af7b9f954989b8fa14c38422ce620addd9395 100755 (executable)
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -219,11 +219,13 @@ class WTBase:
                msg.status.append(stat)
                self.pub_diag.publish(msg)
 
+               current = struct.unpack(">h", i2c_read_reg(0x52, 0x0D, 2))[0]/1000.0
+
                if self.pub_battery.get_num_connections() > 0:
                        batmsg = BatteryState()
                        batmsg.header.stamp = rospy.Time.now()
                        batmsg.voltage = volt
-                       batmsg.current = float('nan')
+                       batmsg.current = current
                        batmsg.charge = float('nan')
                        batmsg.capacity = float('nan')
                        batmsg.design_capacity = 5.0