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 update_position(self):
position = px.Coordinates()
error = px.PositionError()
self.pozyx.doPositioning(
position, self.dimension, self.height, self.algorithm,
remote_id=self.remote_id)
self.pozyx.getPositionError(error)
if self.accept_position(position, error):
return (np.array([position.x, position.y, position.z]) / 1000.0, error)
else:
return None, None
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()
def loop(self):
#Topic 1: PoseWitchCovariance
pwc = PoseWithCovarianceStamped()
pwc.header.stamp = rospy.get_rostime()
pwc.header.frame_id = self.world_frame_id
pwc.pose.pose.position = Coordinates()
pwc.pose.pose.orientation = pypozyx.Quaternion()
cov = pypozyx.PositionError()
status = self.pozyx.doPositioning(pwc.pose.pose.position,
self.dimension, self.height,
self.algorithm, self.tag_device_id)
pozyx.getQuaternion(pwc.pose.pose.orientation, self.tag_device_id)
pozyx.getPositionError(cov, self.tag_device_id)
cov_row1 = [cov.x, cov.xy, cov.xz, 0, 0, 0]
cov_row2 = [cov.xy, cov.y, cov.yz, 0, 0, 0]
cov_row3 = [cov.xz, cov.yz, cov.z, 0, 0, 0]
cov_row4 = [0, 0, 0, 0, 0, 0]
cov_row5 = [0, 0, 0, 0, 0, 0]
cov_row6 = [0, 0, 0, 0, 0, 0]
pwc.pose.covariance = cov_row1 + cov_row2 + cov_row3 + cov_row4 + cov_row5 + cov_row6
#Convert from mm to m
pwc.pose.pose.position.x = pwc.pose.pose.position.x * 0.001
pwc.pose.pose.position.y = pwc.pose.pose.position.y * 0.001
pwc.pose.pose.position.z = pwc.pose.pose.position.z * 0.001
if status == POZYX_SUCCESS:
pub_pose_with_cov.publish(pwc)
#Topic 2: IMU
imu = Imu()
imu.header.stamp = rospy.get_rostime()
imu.header.frame_id = self.tag_frame_id
imu.orientation = pypozyx.Quaternion()
imu.orientation_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.angular_velocity = pypozyx.AngularVelocity()
imu.angular_velocity_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
imu.linear_acceleration = pypozyx.LinearAcceleration()
imu.linear_acceleration_covariance = [0, 0, 0, 0, 0, 0, 0, 0, 0]
pozyx.getQuaternion(imu.orientation, self.tag_device_id)
pozyx.getAngularVelocity_dps(imu.angular_velocity, self.tag_device_id)
pozyx.getLinearAcceleration_mg(imu.linear_acceleration,
self.tag_device_id)
#Convert from mg to m/s2
imu.linear_acceleration.x = imu.linear_acceleration.x * 0.0098
imu.linear_acceleration.y = imu.linear_acceleration.y * 0.0098
imu.linear_acceleration.z = imu.linear_acceleration.z * 0.0098
#Convert from Degree/second to rad/s
imu.angular_velocity.x = imu.angular_velocity.x * 0.01745
imu.angular_velocity.y = imu.angular_velocity.y * 0.01745
imu.angular_velocity.z = imu.angular_velocity.z * 0.01745
pub_imu.publish(imu)
#Topic 3: Anchors Info
for i in range(len(anchors)):
dr = AnchorInfo()
dr.header.stamp = rospy.get_rostime()
dr.header.frame_id = self.world_frame_id
dr.id = hex(anchors[i].network_id)
dr.position.x = (float)(anchors[i].pos.x) * 0.001
dr.position.y = (float)(anchors[i].pos.y) * 0.001
dr.position.z = (float)(anchors[i].pos.z) * 0.001
iter_ranging = 0
while iter_ranging < self.do_ranging_attempts:
device_range = DeviceRange()
status = self.pozyx.doRanging(self.anchors[i].network_id,
device_range, self.tag_device_id)
dr.distance = (float)(device_range.distance) * 0.001
dr.RSS = device_range.RSS
if status == POZYX_SUCCESS:
dr.status = True
self.range_error_counts[i] = 0
iter_ranging = self.do_ranging_attempts
else:
dr.status = False
self.range_error_counts[i] += 1
if self.range_error_counts[i] > 9:
self.range_error_counts[i] = 0
rospy.logerr("Anchor %d (%s) lost", i, dr.id)
iter_ranging += 1
# device_range = DeviceRange()
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# status = self.pozyx.doRanging(self.anchors[i].network_id, device_range)
# dr.distance = (float)(device_range.distance) * 0.001
# dr.RSS = device_range.RSS
# if status == POZYX_SUCCESS:
# dr.status = True
# self.range_error_counts[i] = 0
# else:
# dr.status = False
# self.range_error_counts[i] += 1
# if self.range_error_counts[i] > 9:
# self.range_error_counts[i] = 0
# rospy.logerr("Anchor %d (%s) lost", i, dr.id)
dr.child_frame_id = "anchor_" + str(i)
pub_anchor_info[i].publish(dr)
#Topic 4: PoseStamped
ps = PoseStamped()
ps.header.stamp = rospy.get_rostime()
ps.header.frame_id = self.world_frame_id
ps.pose.position = pwc.pose.pose.position
ps.pose.orientation = pwc.pose.pose.orientation
pub_pose.publish(ps)
#Topic 5: Pressure
pr = FluidPressure()
pr.header.stamp = rospy.get_rostime()
pr.header.frame_id = self.tag_frame_id
pressure = pypozyx.Pressure()
pozyx.getPressure_Pa(pressure, self.tag_device_id)
pr.fluid_pressure = pressure.value
pr.variance = 0
pub_pressure.publish(pr)