def read(self):
rate = rospy.Rate(60) # Hz
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
euler_angles = pypozyx.EulerAngles()
status = self.pozyx.doPositioning(coords, remote_id=self.remote_id)
if status == pypozyx.POZYX_SUCCESS:
self.x = coords.x / 1000.0
self.y = coords.y / 1000.0
self.z = coords.z / 1000.0
status = self.pozyx.getEulerAngles_deg(euler_angles,
remote_id=self.remote_id)
if status == pypozyx.POZYX_SUCCESS:
self.roll = euler_angles.roll * 3.14159 / 180.0
self.pitch = euler_angles.pitch * 3.14159 / 180.0
self.yaw = (90 - euler_angles.heading) * 3.14159 / 180.0
if self.yaw < -3.14159:
self.yaw = self.yaw + (2 * 3.14159)
try: # prevent garbage in console output when thread is killed
rate.sleep()
except rospy.ROSInterruptException:
pass
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_position_euler_pub():
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
vx = 0.0
vy = 0.0
vth = 0.0
rospy.init_node('position_euler_pub')
#pub odom msg for move_base
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
current_time = rospy.Time.now()
last_time = rospy.Time.now()
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()
pozyx.doPositioning(coords, remote_id=remote_id)
position_pub.publish(coords.x, coords.y, coords.z)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
euler_pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
# combine pos and angle info and publish as odom msg
current_time = rospy.Time.now()
# odometry is 6DOF, created quaternion from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, euler_angles.heading)
# publish the transform over tf
odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat,
current_time, "base_footprint",
"odom") #base_link
# publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = "odom"
odom.pose.pose = Pose(Point(coords.x, coords.y, 0.),
Quaternion(*odom_quat))
odom.child_frame_id = "base_footprint" #base_link
odom.twist.twist = Twist(Vector3(vx, vy, 0),
Vector3(0, 0, vth)) # set the velocity
odom_pub.publish(odom)
last_time = current_time
def pozyx_position_euler_pub():
global rcvd_msgs
global rec_x
global rec_y
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/charlie_pose', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
pub1 = rospy.Publisher('/tag_pose0', Point, queue_size=100)
pub2 = rospy.Publisher('/tag_pose1', Point, queue_size=100)
rospy.init_node('position_euler_pub')
rate=rospy.Rate(1)
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()
pozyx.doPositioning(coords, remote_id=remote_id)
pozyx.doPositioning(tags, remote_id=remote_id)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
euler_pub.publish(euler_angles.heading,
euler_angles.roll, euler_angles.pitch)
rospy.loginfo("X: %f Y: %f" , tags.x, tags.y)
rospy.loginfo(euler_angles)
pub1.publish(tags.x/1000.0,tags.y/1000.0,tags.z/1000.0)
pub2.publish(tags.x/1000.0, tags.y/1000.0,tags.z/1000.0)
position_pub.publish(coords.x/1000.0, coords.y/1000.0, coords.z/1000.0)
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
)
rate.sleep()
def pozyx_euler_pub():
pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100)
rospy.init_node('pozyx_euler_pub')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id)
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
def pozyx_euler_pub():
pub = rospy.Publisher('pozyx_euler_angles', EulerAngles, queue_size=100)
rospy.init_node('pozyx_euler_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():
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles, remote_id=remote_id)
rospy.loginfo(euler_angles)
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
def pozyx_position_euler_pub():
position_pub = rospy.Publisher('pozyx_positioning',
Point32,
queue_size=100)
euler_pub = rospy.Publisger('pozyx_euler_angles',
EulerAngles,
queue_size=100)
rospy.init_node('position_euler_pub')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
pub.publish(coords.x, coords.y, coords.z)
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
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 pozyx_position_euler_pub():
#position_pub = rospy.Publisher(
# 'pozyx_positioning', Point32, queue_size=100)
position_pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
#euler_pub = rospy.Publisher(
# 'pozyx_euler_angles', EulerAngles, queue_size=100)
euler_pub = rospy.Publisher('/orientation', Point, queue_size=100)
vx = 0.0
vy = 0.0
vth = 0.0
rospy.init_node('position_euler_pub') #PROVA
#pub odom msg for move_base
odom_pub = rospy.Publisher("odom", Odometry, queue_size=50)
odom_broadcaster = tf.TransformBroadcaster()
current_time = rospy.Time.now()
last_time = rospy.Time.now()
#rospy.init_node('position_euler_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():
#################### COORDS ####################
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
# convert from mm to m
#rospy.loginfo("POS_1: %s m" % (str(coords)))
coords.x = coords.x / 1000.0
coords.y = coords.y / 1000.0
coords.z = coords.z / 1000.0
#rospy.loginfo("POS_1: %s m" % (str(coords)))
'''
if records < n_samples:
coords.x += coords.x
coords.y += coords.y
coords.z += coords.z
records = 'done'
continue
elif records == 'done':
coords.x /= n_samples
coords.y /= n_samples
coords.z /= n_samples
records = 'finish'
continue
'''
position_pub.publish(coords.x, coords.y, coords.z)
#################### ANGLES #####################
euler_angles = pypozyx.EulerAngles()
pozyx.getEulerAngles_deg(euler_angles)
# convert from degrees to radians ( *3.14/180 )
euler_angles.heading = numpy.deg2rad(euler_angles.heading)
euler_angles.roll = numpy.deg2rad(euler_angles.roll)
euler_angles.pitch = numpy.deg2rad(euler_angles.pitch)
rospy.loginfo("POS: %s, ANGLES: %s" % (str(coords), str(euler_angles)))
euler_pub.publish(euler_angles.heading, euler_angles.roll,
euler_angles.pitch)
#################### PUBLISHING ODOM TF ################
# combine pos and angle info and publish as odom msg
current_time = rospy.Time.now()
# odometry is 6DOF, created quaternion from yaw
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, euler_angles.heading)
# publish the transform over tf
odom_broadcaster.sendTransform((coords.x, coords.y, 0.), odom_quat,
current_time, tag_frame_ID,
UWB_frame_ID)
# publish the odometry message over ROS
odom = Odometry()
odom.header.stamp = current_time
odom.header.frame_id = UWB_frame_ID
odom.pose.pose = Pose(Point(coords.x, coords.y, 0.),
Quaternion(*odom_quat))
odom.child_frame_id = tag_frame_ID
odom.twist.twist = Twist(Vector3(vx, vy, 0),
Vector3(0, 0, vth)) # set the velocity
odom_pub.publish(odom)
last_time = current_time