From db645690cb11053418bc17861d335966ba3c4682 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Wed, 13 May 2020 19:19:57 +0200 Subject: [PATCH] remove ir sensors --- scripts/wt_node.py | 14 -------------- urdf/wild_thumper.urdf.xacro | 2 -- 2 files changed, 16 deletions(-) diff --git a/scripts/wt_node.py b/scripts/wt_node.py index 03d61be..2969d61 100755 --- a/scripts/wt_node.py +++ b/scripts/wt_node.py @@ -37,8 +37,6 @@ class WTBase: self.pub_range_fwd_left = rospy.Publisher("range_forward_left", Range, queue_size=16) self.pub_range_fwd_right = rospy.Publisher("range_forward_right", Range, queue_size=16) self.pub_range_bwd = rospy.Publisher("range_backward", Range, queue_size=16) - self.pub_range_left = rospy.Publisher("range_left", Range, queue_size=16) - self.pub_range_right = rospy.Publisher("range_right", Range, queue_size=16) self.pub_battery = rospy.Publisher("battery", BatteryState, queue_size=16) self.cmd_vel = None self.cur_vel = (0, 0) @@ -313,18 +311,6 @@ class WTBase: msg.range = dist pub.publish(msg) - def get_dist_left(self): - if self.pub_range_left.get_num_connections() > 0: - dist = self.get_dist_ir(0x1) - if dist > -67: - self.send_range(self.pub_range_left, "ir_left", Range.INFRARED, 30.553/(dist - -67.534), 0.04, 0.3, 1) - - def get_dist_right(self): - if self.pub_range_right.get_num_connections() > 0: - dist = self.get_dist_ir(0x3) - if dist > 69: - self.send_range(self.pub_range_right, "ir_right", Range.INFRARED, 17.4/(dist - 69), 0.04, 0.3, 1) - def get_dist_forward_left(self): if self.pub_range_fwd_left.get_num_connections() > 0: dist = self.read_dist_srf(0x15) diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro index 9d6dd8e..9c929e5 100644 --- a/urdf/wild_thumper.urdf.xacro +++ b/urdf/wild_thumper.urdf.xacro @@ -279,8 +279,6 @@ - - -- 2.39.5