def pozyx_pose_pub():
pub = rospy.Publisher('amcl_pose', PoseStamped, vicon_cb, queue_size=100)
rospy.init_node('pozyx_pose_node')
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():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
pose.pose.position.x = coords.x
pose.pose.position.y = coords.y
pose.pose.position.z = coords.z
pose.pose.orientation.x = quat.x
pose.pose.orientation.y = quat.y
pose.pose.orientation.z = quat.z
pose.pose.orientation.w = quat.w
pub.publish(pose)
def pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
pubStamped = rospy.Publisher('pozyx_pose_stamped',
PoseStamped,
queue_size=40)
rospy.init_node('pozyx_pose_node')
h = std_msgs.msg.Header()
h.frame_id = "pozyx"
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
h.stamp = rospy.Time.now()
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
pubStamped.publish(
h,
Pose(
Point(
float(coords.x) / 1000,
float(coords.y) / 1000,
float(coords.z) / 1000),
Quaternion(quat.x, quat.y, quat.z, quat.w)))
def pozyx_pose_pub():
pub = rospy.Publisher('/charlie_pose',
PoseWithCovarianceStamped,
vicon_cb,
queue_size=1000)
pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=1000)
pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=1000)
rospy.init_node('pozyx_pose_node')
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():
coords = pypozyx.Coordinates()
tags = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
pose.pose.pose.position.x = coords.x / 1000.0
pose.pose.pose.position.y = coords.y / 1000.0
pose.pose.pose.position.z = coords.z / 1000.0
# ho messo come messaggio PoseWithCovarianceStamped ed ho aggiunto la covarianza a mano, quindi per accedere alle variabili di pos e orient bisogna fare pose.pose.pose, se fosse solo PoseStamped basta pose.pose
pose.pose.covariance[0] = 0.01
pose.pose.covariance[7] = 0.01
pose.pose.covariance[14] = 99999
pose.pose.covariance[21] = 99999
pose.pose.covariance[28] = 99999
pose.pose.covariance[35] = 0.01
tags.x = coords.x / 1000.0
tags.y = coords.y / 1000.0
tags.z = coords.z / 1000.0
pose.pose.pose.orientation.x = quat.x
pose.pose.pose.orientation.y = quat.y
pose.pose.pose.orientation.z = quat.z
pose.pose.pose.orientation.w = quat.w
rospy.loginfo("POS: %s, QUAT: %s" % (str(tags), str(quat)))
pub.publish(pose)
pub1.publish(Point(tags.x, tags.y, tags.z))
pub2.publish(Point(tags.x, tags.y, tags.z))
euler_angles = pypozyx.EulerAngles()
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
tf_quat = tf.transformations.quaternion_from_euler(
0, 0, -euler_angles.heading)
br = tf.TransformBroadcaster()
br.sendTransform((coords.x / 1000.0, coords.y / 1000.0, 0), tf_quat,
rospy.Time.now(), tag0_frame_ID, UWB_frame_ID)
def pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
rospy.init_node('pozyx_pose_node')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
def pozyx_pose_pub(self):
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
self.pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=self.remote_id)
self.pozyx.getQuaternion(quat, remote_id=self.remote_id)
pos = [coords.x/1000.0,coords.y/1000.0,coords.z/1000.0]
self.quat = self.normalizeQuaternion(quat)
self.smoothData(pos)
self.tb.sendTransform(self.pos,
self.quat,
rospy.Time.now(),
"pozyx",
"map"
)
def pozyx_pose_pub():
pub = rospy.Publisher('pozyx_pose', Pose, queue_size=40)
rospy.init_node('pozyx_pose_node')
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():
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
rospy.loginfo("POS: %s, QUAT: %s" % (str(coords), str(quat)))
pub.publish(Point(coords.x, coords.y, coords.z),
Quaternion(quat.x, quat.y, quat.z, quat.w))
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)
def pozyx_pose_pub():
# ------------------ PUBLISHERS ------------------ #
pose_pub_0 = rospy.Publisher(tag0_topic_ID, PoseStamped, queue_size=100)
pose_pub_1 = rospy.Publisher(tag1_topic_ID, PoseStamped, queue_size=100)
rospy.init_node('pozyx_pose_node_2_tags')
# Inizializzazione delle porte seriali, cerca automaticament 2 tag (se si lascia una sola tag connessa non funziona e richiede di connettere un dispositivo Pozyx)
try:
serial_port_0 = pypozyx.get_pozyx_ports()[0]
#serial_port_1 = pypozyx.get_pozyx_ports()[1]
pozyx_0 = pypozyx.PozyxSerial(serial_port_0)
#pozyx_1 = pypozyx.PozyxSerial(serial_port_1)
######################################################################
"""
Verifichiamo che il tag posizionato in testa corrisponda effettivamente a pozyx_0 da cui leggeremo
altrimenti i due tag vengono scambiati. Questo perche' ad ogni riavvio le porte seriali vengono reinizializzate
system_details = DeviceDetails()
pozyx_0.getDeviceDetails(system_details, remote_id=None) #Serve per ottenere l'ID della tag che in questo momento viene vista come pozyx_0 e che dovrebbe essere la testa
tag_ID = system_details.id
print("La testa dovrebbe essere con id 0x%0.4x, attualmente misurata e' id 0x%0.4x" % (tag0_ID, system_details.id))
"""
# Se l'ID trovato non corrisponde a quello desiderato per la tag frontale, i due vengono scambiati
'''
if tag_ID != tag0_ID :
print("Aggiusto le tag")
pozyx_0_tmp = pozyx_0
pozyx_0 = pozyx_1
pozyx_1 = pozyx_0_tmp
system_details = DeviceDetails()
pozyx_0.getDeviceDetails(system_details, remote_id=None)
print("Adesso la testa e' 0x%0.4x" % (system_details.id))
'''
######################################################################
except:
rospy.loginfo("Pozyx not connected")
return
# Stampa le porte seriali attive con tag connesse, poi attende un comando per iniziare a pubblicare
rospy.loginfo("Using tag connected to : \n %s", serial_port_0)
raw_input("Press any key to continue\n")
while not rospy.is_shutdown():
# Inizializzazione delle variabili
coords = pypozyx.Coordinates()
quat = pypozyx.Quaternion()
euler_angles = pypozyx.EulerAngles()
# Preparo una lista contenente le due tag da cui leggere
pozyx_tag_list = [pozyx_0]
# Leggo da una tag alla volta
for pozyx in pozyx_tag_list :
pozyx.doPositioning(coords, pypozyx.POZYX_3D, remote_id=remote_id)
pozyx.getQuaternion(quat, remote_id=remote_id)
pozyx.getEulerAngles_deg(euler_angles)
pose.header.frame_id = UWB_frame_ID
pose.header.stamp = rospy.Time.now()
# Converto da [mm] a [m]
pose.pose.position.x = coords.x / 1000.0
pose.pose.position.y = coords.y / 1000.0
pose.pose.position.z = coords.z / 1000.0
pose.pose.orientation.x = quat.x
pose.pose.orientation.y = quat.y
pose.pose.orientation.z = quat.z
pose.pose.orientation.w = quat.w
# Converto da gradi a radianti
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
euler_angles.roll = numpy.deg2rad(euler_angles.roll)
euler_angles.pitch = numpy.deg2rad(euler_angles.pitch)
# Il quaternione per la trasformazione tiene conto solo della rotazione su z
tf_quat = tf.transformations.quaternion_from_euler(0, 0, -euler_angles.heading)
# in base al tag dal quale si sta leggendo, vengono pubblicate posa e tf sui rispettivi topic
pose_pub_0.publish(pose)
br_0 = tf.TransformBroadcaster()
br_0.sendTransform((pose.pose.position.x, pose.pose.position.y, 0),
tf_quat,
rospy.Time.now(),
tag0_frame_ID,
UWB_frame_ID
)
rospy.loginfo("tag_0|| POS-> X: %f Y: %f Z: %f \t | ANGLE-> roll: %f pitch: %f yaw: %f ||" , pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, euler_angles.roll, euler_angles.pitch, euler_angles.heading)
def update_orientation(self):
quat = px.Quaternion()
self.pozyx.getQuaternion(quat)
return [quat.x, quat.y, quat.z, quat.w]
