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)
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():
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()
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)