def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub_0x6940')
r = rospy.Rate(10)
try:
#pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
pozyx = pypozyx.PozyxSerial('/dev/ttyACM0')
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
# temporary fix for the issue
pozyx.checkForFlag(pypozyx.definitions.bitmasks.POZYX_INT_MASK_RX_DATA,
pypozyx.POZYX_DELAY_INTERRUPT)
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS, str(destination_id))
rospy.loginfo(device_range)
r.sleep()
else:
error_code = pypozyx.SingleRegister()
pozyx.getErrorCode(error_code)
rospy.loginfo('ERROR: RANGING, error code %s' % error_code)
if str(error_code) in '0x1':
rospy.loginfo('ERROR: RANGING, killing publisher')
return
def init():
global msg, device_range, range_pub
msg = Range()
msg.radiation_type = 2
msg.field_of_view = 3.1415
msg.min_range = 0.2
msg.max_range = 10.0 # depends on the configuration, 300m on datasheet, 10m reported by users
device_range = pypozyx.DeviceRange()
rospy.init_node('pozyx_pose_node')
def pozyx_range():
pub = rospy.Publisher('pozyx_range', DeviceRange, queue_size=100)
rospy.init_node('range_pub')
try:
serial_port = pypozyx.get_first_pozyx_serial_port()
if serial_port == None:
rospy.loginfo("Pozyx not connected")
return
pozyx = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Could not connect to pozyx")
return
# get network id of connected pozyx
self_id = pypozyx.NetworkID()
if pozyx.getNetworkId(self_id) != pypozyx.POZYX_SUCCESS:
rospy.loginfo("Could not get network id")
return
# discover all other pozyx
pozyx.doDiscoveryAll()
devices_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(devices_size)
devices = pypozyx.DeviceList(devices_size)
pozyx.getDeviceIds(devices)
for i in range(devices_size[0]):
rospy.loginfo("Found pozyx device with the id {}".format(
pypozyx.NetworkID(devices[i])))
# publish data
while not rospy.is_shutdown():
for i in range(devices_size[0]):
device_range = pypozyx.DeviceRange()
remote_id = pypozyx.NetworkID(devices[i])
if pozyx.doRanging(self_id[0], device_range, remote_id[0]):
h = Header()
h.stamp = rospy.Time.from_sec(device_range[0] / 1000)
pub.publish(h, remote_id[0], device_range[1], device_range[2])
else:
rospy.loginfo("Error while ranging")
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(str(device_range))
else:
rospy.loginfo('ERROR: RANGING')
def rangeTo(self, from_id, to_id):
"""
Gets the range from one pozyx device in to another
Paramters:
from_id (number): the id of the device to measure from
to_id (number): the id of the device to measure to
Returns:
(DeviceRange): the range between the devices
"""
device_range = pypozyx.DeviceRange()
status = self.local.doRanging(from_id, device_range, to_id)
self._check_status("ranging {:02x} -> {:02x}".format(from_id, to_id),
status)
return device_range
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
# MOD @ 12-08-2020
serial_port = pypozyx.get_first_pozyx_serial_port()
pozyx = pypozyx.PozyxSerial(serial_port)
# BKP -> pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(device_range)
else:
rospy.loginfo('ERROR: RANGING')
def __init__(self, pozyx, dt, ranging_protocol, robot_list, tag_pos, robot_number, alpha, noise):
self.pozyx = pozyx
self.ranging_protocol = ranging_protocol
self.tag_pos = tag_pos
self.robot_number = robot_number
self.distance_1 = pzx.DeviceRange()
self.distance_2 = pzx.DeviceRange()
self.distance_3 = pzx.DeviceRange()
self.distance_4 = pzx.DeviceRange()
self.A = robot_list[robot_number]['left']
self.B = robot_list[robot_number]['right']
if robot_number == 1:
self.C = robot_list[2]['left']
self.D = robot_list[2]['right']
elif robot_number == 2:
self.C = robot_list[1]['left']
self.D = robot_list[1]['right']
self.distance_prev_1 = 0
self.distance_prev_2 = 0
self.distance_prev_3 = 0
self.distance_prev_4 = 0
self.f1 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f1.P = 1
self.f1.H = np.array([[1.]])
self.f1.F = np.array([[1.]])
self.f1.B = np.array([[1.]])
self.f1.Q = noise
self.f1.R = 0.1
self.f1.alpha = alpha
self.f2 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f2.P = 1
self.f2.H = np.array([[1.]])
self.f2.F = np.array([[1.]])
self.f2.B = np.array([[1.]])
self.f2.Q = noise
self.f2.R = 0.1
self.f2.alpha = alpha
self.f3 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f3.P = 1
self.f3.H = np.array([[1.]])
self.f3.F = np.array([[1.]])
self.f3.B = np.array([[1.]])
self.f3.Q = noise
self.f3.R = 0.1
self.f3.alpha = alpha
self.f4 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f4.P = 1
self.f4.H = np.array([[1.]])
self.f4.F = np.array([[1.]])
self.f4.B = np.array([[1.]])
self.f4.Q = noise
self.f4.R = 0.1
self.f4.alpha = alpha
self.f5 = KalmanFilter(dim_x=1, dim_z=1, dim_u=1)
self.f5.P = 1
self.f5.H = np.array([[1.]])
self.f5.F = np.array([[1.]])
self.f5.B = np.array([[1.]])
self.f5.Q = 0.1
self.f5.R = 0.001
self.f5.alpha = 1
self.pozyx.setRangingProtocol(self.ranging_protocol)
self.br = tf.TransformBroadcaster()
