X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=scripts%2Fwt_node.py;h=bffabce5d178bb9fec2954587565c3363fa2fea3;hp=2969d613c7f11a06e81ac6efe17d740f059cf80f;hb=eec61b85016f4b8acc989b2aecf5c38f838cb850;hpb=db645690cb11053418bc17861d335966ba3c4682 diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 2969d61..bffabce 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -58,7 +58,6 @@ class WTBase: sleep(3) # wait 3s for ros to register and establish all subscriber connections before sending reset diag reset_val = self.get_reset() rospy.loginfo("Reset Status: 0x%x" % reset_val) - ir_count = 0 sonar_count = 0 i2c_write_reg(0x50, 0xA4, struct.pack("B", 1)) # enable Watchdog while not rospy.is_shutdown(): @@ -69,13 +68,6 @@ class WTBase: self.get_odom() self.get_power() - if ir_count == 0: - self.get_dist_left() - ir_count+=1 - else: - self.get_dist_right() - ir_count=0 - if sonar_count == 0: self.get_dist_forward_left() self.update_dist_backward()