def loop(self):
"""Performs positioning and displays/exports the results."""
position = Coordinates()
range = DeviceRange()
status = self.pozyx.doPositioning(position,
self.dimension,
self.height,
self.algorithm,
remote_id=self.remote_id)
if status == POZYX_SUCCESS:
if (self.id == 0):
rospy.loginfo("id tag: " + str(self.id))
print(
str(position.x) + '\t' + str(position.y) + '\t' +
str(position.z) + '\t')
self.Pointmsg.x = position.x
self.Pointmsg.y = position.y
self.Pointmsg.z = position.z
if (self.Pointmsg.x is not 0) and (self.Pointmsg.y
is not 0) and (self.Pointmsg.z
is not 0):
self.pub.publish(self.Pointmsg)
count = 0
#for anchor in self.anchor_ids:
# self.pozyx.getDeviceRangeInfo(anchor, range, remote_id)
# out_file.write(str(range.distance)+'\t'+str(range.RSS)+'\t'+str(range.timestamp)+'\t')
# print (str(range.timestamp))
# count = count + 1
#out_file.write('\n')
else:
self.printPublishErrorCode("positioning")
def loop(self):
"""Perform ranging and sets the LEDs accordingly."""
device_range = DeviceRange()
status = self.pozyx.doRanging(self.destination_id, device_range,
self.remote_id)
if status == POZYX_SUCCESS:
print(str(device_range))
d = str(device_range)
d = d.split(', ')
t = int(d[0].replace(' ms', ''))
dist = int(d[1].replace(' mm', ''))
dbm = int(d[2].replace(' dBm', ''))
vector = np.zeros((1, 3))
vector = np.array(([t, dist, dbm]))
# if self.ledControl(device_range.distance) == POZYX_FAILURE:
# print("ERROR: setting (remote) leds")
return vector
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
return ([None, None, None])
def loop(self):
for i in range(len(self.destination_id)):
device_range = DeviceRange()
self.pozyx.doRanging(self.destination_id[i], device_range,
self.remote_id)
distance = (float)(device_range.distance) * 0.001
if i == 0:
distance_1 = distance
elif i == 1:
distance_2 = distance
elif i == 2:
distance_3 = distance
elif i == 3:
distance_4 = distance
#Distance 1
if distance_1 == 0:
distance_1 = self.distance_prev_1
self.f1.update(distance_1)
self.f1.predict()
#Distance 2
if distance_2 == 0:
distance_2 = self.distance_prev_2
self.f2.update(distance_2)
self.f2.predict()
#Distance 3
if distance_3 == 0:
distance_3 = self.distance_prev_3
self.f3.update(distance_3)
self.f3.predict()
#Distance 4
if distance_4 == 0:
distance_4 = self.distance_prev_4
self.f4.update(distance_4)
self.f4.predict()
self.distance_prev_1 = distance_1
self.distance_prev_2 = distance_2
self.distance_prev_3 = distance_3
self.distance_prev_4 = distance_4
self.distances = [distance_1, distance_2, distance_3, distance_4]
self.distances_kf = [
self.f1.x[0], self.f2.x[0], self.f3.x[0], self.f4.x[0]
]
rospy.loginfo(self.distances)
return self.trilaterate3D()
def get_ranges(self):
range = DeviceRange()
success = False
for idx, anchor in enumerate(self.anchors):
status = self.pozyx.doRanging(anchor.network_id, range,
self.remote_id)
if status == POZYX_SUCCESS:
self.send_beacon_distance(idx, range.distance)
success = True
if success == False:
print("Failed to get any ranges")
def loop(self):
"""Performs ranging and sets the LEDs accordingly"""
device_range = DeviceRange()
status = self.pozyx.doRanging(self.destination_id, device_range,
self.remote_id)
if status == POZYX_SUCCESS:
print(device_range)
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
def loop(self):
range = DeviceRange()
stat = self.pozyx.doRanging(self.dest, range, self.remote)
if (stat == POZYX_SUCCESS):
print(range)
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
print("ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
print("ERROR Ranging, couldn't retrieve local error")
def range_callback(self):
"""Do ranging periodically, and publish visualizasion_msg MarkerArray"""
for tag_id in self.tag_ids:
for anchor in self.anchors:
device_range = DeviceRange()
status = self.pozyx.doRanging(anchor.network_id, device_range,
tag_id)
if status == POZYX_SUCCESS:
self.publishMarkerArray(device_range.distance, anchor)
# self.get_logger().info(f"{device_range.distance}")
else:
error_code = SingleRegister()
status = self.pozyx.getErrorCode(error_code)
if status == POZYX_SUCCESS:
self.get_logger().error(
"ERROR Ranging, local %s" %
self.pozyx.getErrorMessage(error_code))
else:
self.get_logger().error(
"ERROR Ranging, couldn't retrieve local error")
self.doPositioning()
def loop(self):
device_range = DeviceRange()
status = self.pozyx.doRanging(self.destination_id, device_range,
self.remote_id)
dis = device_range.distance
return dis
def setup(self):
device_range = DeviceRange()
#self.pozyx.printDeviceInfo(self.remote_id)
self.pozyx.setRangingProtocol(self.protocol, self.remote_id)
def __init__(self,sim,p):
rospy.init_node('pozyx')
self.sim = sim
self.p = p
if not self.sim and self.p is None:
rospy.loginfo("No pozyx device!")
self.denoise = False
try:
rospy.get_param('anchor0')
except KeyError:
rospy.loginfo("who is my anchor0?")
return
try:
rospy.get_param('anchor1')
except KeyError:
rospy.loginfo("who is my anchor1?")
return
try:
rospy.get_param('anchor2')
except KeyError:
rospy.loginfo("who is my anchor2?")
return
if self.sim:
self.truestate_sub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.truestate_callback)
self.anchor0_sub = rospy.Subscriber('/'+rospy.get_param('anchor0')+'/pozyx_position', PoseStamped, self.anchor0_callback)
self.anchor1_sub = rospy.Subscriber('/'+rospy.get_param('anchor1')+'/pozyx_position', PoseStamped, self.anchor1_callback)
self.anchor2_sub = rospy.Subscriber('/'+rospy.get_param('anchor2')+'/pozyx_position', PoseStamped, self.anchor2_callback)
self.odom_sub = rospy.Subscriber('odom', Odometry, self.odom_callback)
# subscribe to the OBSTACLE_DETECTOR for the sake of determining if there is an obstacle obstructing pozyx
self.laser_sub = rospy.Subscriber('obstacle_detect', LaserScan, self.laser_callback)
self.pub = rospy.Publisher(rospy.get_namespace()+'pozyx_position', PoseStamped, queue_size=10)
# for simulation
self.gazebo_states, self.scan_reading, self.truepose = None, None, None
# for everything
self.anchor0, self.anchor1, self.anchor2 = None, None, None
# for real pozyx
if not self.sim:
self.networkId = NetworkID()
self.p.getNetworkId(self.networkId)
self.anchor_device = {"tb3_2":0x6858, "tb3_3":0x685a, "tb3_0":0x685e, "tb3_1":0x6816}
self.ranges = {}
for tb in self.anchor_device.keys():
if self.anchor_device[tb] != self.networkId.value:
self.ranges[self.anchor_device[tb]] = DeviceRange()
self.initialpose = PoseStamped()
self.initialpose.header.stamp = rospy.Time.now()
self.initialpose.header.frame_id = rospy.get_namespace()+"base_scan"
self.initialpose.pose.position.x, self.initialpose.pose.position.y, self.initialpose.pose.position.z = float(sys.argv[2]), float(sys.argv[3]), 0
q = quaternion_from_euler(0, 0, float(sys.argv[4]))
self.initialpose.pose.orientation.x, self.initialpose.pose.orientation.y, self.initialpose.pose.orientation.z, self.initialpose.pose.orientation.w = q[0], q[1], q[2], q[3]
rospy.loginfo("pozyx: given initial pose (%f, %f)", self.initialpose.pose.position.x, self.initialpose.pose.position.y)
self.odompose = PoseStamped()
self.skip_jump_avoidance = (rospy.get_namespace() == "/tb3_3/")
while not rospy.is_shutdown():
if not (self.anchor0 and self.anchor1 and self.anchor2):
self.set_initial_position()
else:
rospy.logdebug("RECEIVED ALL ANCHOR POSITIONS")
self.set_pozyx_position()
rospy.sleep(0.02) # to do: choose rate