def update(self, event):
try:
if self.enable_position:
position, error = self.update_position()
else:
position = [0, 0, 0]
except (PozyxExceptionCommQueueFull, PozyxExceptionOperationQueueFull,
PozyxExceptionTimeout) as e:
rospy.logwarn('%s: will sleep for 5 s', e.message)
rospy.sleep(5)
return
except PozyxException as e:
rospy.logerr('%s', e.message)
return
try:
if self.enable_orientation:
# !! pozyx orientation is not absolute but relative to the orientation at start
q = self.update_orientation()
self.orientation = self.quaternion_in_map_frame(q)
else:
self.orientation = [0, 0, 0, 1]
except Exception as e:
rospy.logerr(e.__class__)
return
if position is not None:
self.publish_pose(position, self.orientation)
if self.enable_raw_sensors:
sensor_data = px.SensorData()
try:
self.pozyx.getAllSensorData(sensor_data, self.remote_id)
except Exception as e:
rospy.logerr(e)
return
self.publish_sensor_data(sensor_data)
def update(self, event):
if not self.ready:
return
try:
if self.enable_position:
position, error = self.update_position()
else:
position = [0, 0, 0]
error = None
# except (PozyxExceptionTimeout) as e:
# # PozyxExceptionCommQueueFull, PozyxExceptionOperationQueueFull
# rospy.logwarn('%s: %s. Will sleep for 1 s', e.__class__.__name__, e.message)
# rospy.sleep(1)
# self.ps.append(0)
# return
except PozyxException as e:
log_pozyx_exception(e)
self.ps.append(0)
self.reset_or_exit()
return
if position is not None:
self.publish_pose(position, error=error)
if self.enable_raw_sensors:
sensor_data = px.SensorData()
try:
self.pozyx.getAllSensorData(sensor_data, self.remote_id)
except PozyxException as e:
log_pozyx_exception(e)
self.reset_or_exit()
return
self.publish_sensor_data(sensor_data)
def run(self):
position = px.Coordinates()
error = px.PositionError()
sensor_data = px.SensorData()
pozyx = self.driver.pozyx
interrupt = px.SingleRegister()
while not rospy.is_shutdown():
try:
if pozyx.checkForFlag(self.flag, self.DELAY, interrupt):
if self.driver.enable_position and interrupt.data[
0] & bm.POZYX_INT_STATUS_POS:
pozyx.getCoordinates(position)
pozyx.getPositionError(error)
if self.driver.accept_position(position, error):
x = np.array([position.x, position.y, position.z
]) / 1000.0
self.driver.publish_pose(x)
if (self.driver.enable_raw_sensors
and interrupt.data[0] & bm.POZYX_INT_STATUS_IMU):
pozyx.getAllSensorData(sensor_data)
self.driver.publish_sensor_data(sensor_data)
rospy.sleep(self.period)
except PozyxExceptionTimeout:
pass
except PozyxException as e:
rospy.logerr(e.message)
pozyx.ser.reset_output_buffer()
pozyx.ser.reset_input_buffer()
rospy.sleep(1)
def run(self):
position = px.Coordinates()
error = px.PositionError()
sensor_data = px.SensorData()
pozyx = self.driver.pozyx
interrupt = px.SingleRegister()
while not rospy.is_shutdown():
try:
if pozyx.checkForFlag(self.flag, self.period, interrupt):
if self.driver.enable_position and interrupt.data[0] & bm.POZYX_INT_STATUS_POS:
pozyx.getCoordinates(position)
pozyx.getPositionError(error)
if self.driver.accept_position(position, error):
x = np.array([position.x, position.y, position.z]) / 1000.0
self.driver.publish_pose(x, error=error)
if (self.driver.enable_raw_sensors and
interrupt.data[0] & bm.POZYX_INT_STATUS_IMU):
pozyx.getAllSensorData(sensor_data)
self.driver.publish_sensor_data(sensor_data)
# except PozyxExceptionTimeout as e:
# rospy.logwarn('%s: %s', e.__class__.__name__, e.message)
except PozyxException as e:
log_pozyx_exception(e)
# pozyx.ser.reset_output_buffer()
# pozyx.ser.reset_input_buffer()
# rospy.sleep(1)
self.driver.reset()
finally:
rospy.sleep(self.period * 0.5)
def init():
global pozyx_coords, pozyx_coords_err, pozyx_sensors
global ros_pose, ros_mag, ros_imu, local_device, lock
global height, algorithm, remote, serial_port, posehz, imuhz
rospy.init_node('pozyx_pose_node')
rospy.Subscriber("pozyx/height", Int16, height_cb)
serial_port = rospy.get_param("port", pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
# 0 is raw, 1 is fused but not yet implemented, 2 is filtered
algorithm = rospy.get_param("algorithm", 2)
remote = rospy.get_param("remote", None)
height = rospy.get_param("height", 100)
posehz = rospy.Rate(rospy.get_param("posehz", 2))
imuhz = rospy.Rate(rospy.get_param("imuhz", 100))
ros_pose = PoseWithCovarianceStamped()
ros_mag = MagneticField()
ros_imu = Imu()
ros_pose.header.frame_id = "pozyx"
ros_mag.header.frame_id = "pozyx"
ros_imu.header.frame_id = "pozyx"
pozyx_coords = pypozyx.Coordinates()
pozyx_coords_err = pypozyx.PositionError()
pozyx_sensors = pypozyx.SensorData()
imu_thread = Thread(target=imu_publisher)
coord_thread = Thread(target=coordinate_publisher)
lock = Lock()
imu_thread.start()
coord_thread.start()
rospy.spin()