def set_anchor_configuration():
global anchors, tags_id
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
serial_port = rospy.get_param("port",
pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Pozyx not connected")
return
for tag in tags_id:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO,
len(anchors),
remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rospy.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
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 start(self):
try:
print(pypozyx.get_serial_ports()[1].device)
self.pozyx = pypozyx.PozyxSerial(
pypozyx.get_serial_ports()[1].device)
whoami = pypozyx.SingleRegister()
self.pozyx.getWhoAmI(whoami)
rospy.loginfo(
"Pozyx WhoAmI {0}".format(whoami)) # will return 0x43
# self.setNewId(0xA003, None)
# rospy.sleep(1)
networkid = pypozyx.NetworkID()
self.pozyx.getNetworkId(networkid)
rospy.loginfo("Network ID {0}".format(networkid))
self.pozyx.setPositionFilter(
pypozyx.PozyxConstants.FILTER_TYPE_NONE, 0)
# self.pozyx.setPositionFilter(pypozyx.PozyxConstants.FILTER_TYPE_MOVING_MEDIAN, 10)
# self.setAnchorsManual(save_to_flash=False)
self.setAnchorsManual(save_to_flash=True)
self.pozyx.printDeviceInfo()
self.printPublishConfigurationResultMore(None)
except rospy.ROSException as e:
rospy.loginfo("Pozyx not connected: {0}", e)
return
self.read_thread = Thread(target=self.read, args=())
self.read_thread.daemon = True
self.read_thread.start()
rospy.sleep(3) # wait to get stable pozyx position
def set_anchor_configuration():
tag_ids = [None]
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
pozyx.doDiscovery(discovery_type=POZYX_DISCOVERY_TAGS_ONLY)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
tag_ids += device_list.data
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(POZYX_ANCHOR_SEL_AUTO,
len(anchors),
remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rosply.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
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)
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_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub_0x6940')
r = rospy.Rate(10)
try:
#pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
pozyx = pypozyx.PozyxSerial('/dev/ttyACM0')
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
# temporary fix for the issue
pozyx.checkForFlag(pypozyx.definitions.bitmasks.POZYX_INT_MASK_RX_DATA,
pypozyx.POZYX_DELAY_INTERRUPT)
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS, str(destination_id))
rospy.loginfo(device_range)
r.sleep()
else:
error_code = pypozyx.SingleRegister()
pozyx.getErrorCode(error_code)
rospy.loginfo('ERROR: RANGING, error code %s' % error_code)
if str(error_code) in '0x1':
rospy.loginfo('ERROR: RANGING, killing publisher')
return
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_positioning_pub():
pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100)
rospy.init_node('positioning_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(Point32(coords.x, coords.y, coords.z))
def __init__(self, anchors, tags, serial_port=None):
if (serial_port is None):
serial_port = pypozyx.get_first_pozyx_serial_port()
assert (serial_port is not None, "Could not find local pozyx device")
self.local = pypozyx.PozyxSerial(serial_port)
# get local id
local_id = pypozyx.NetworkID()
status = self.local.getNetworkId(local_id)
self._check_status("get network id", status)
self.local_id = local_id[0]
# check for nessary devices and set coordinates
self._find_devices()
device_list = self._get_device_list()
device_list.append(self.local_id)
for device in anchors:
id = device.network_id
cord = device.pos
if id in device_list:
status = self.local.setCoordinates(
cord, None if id == self.local_id else id)
self._check_status("set coordinates", status)
else:
raise Exception(
"failed to find a pozyx device with the id {:02x}".format(
id))
for id in tags:
if id not in device_list:
raise Exception(
"failed to find a pozyx device with the id {:02x}".format(
id))
# remove any unknown devices
anchor_ids = map(lambda device: device.network_id, anchors)
for id in device_list:
if (id not in anchor_ids) and (id not in tags):
# TODO: log removal of unknown device
self.local.removeDevice(id)
# store lists of anchors and tags
self.anchors = anchor_ids
self.tags = tags
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_range():
pub = rospy.Publisher('pozyx_range', DeviceRange, queue_size=100)
rospy.init_node('range_pub')
try:
serial_port = pypozyx.get_first_pozyx_serial_port()
if serial_port == None:
rospy.loginfo("Pozyx not connected")
return
pozyx = pypozyx.PozyxSerial(serial_port)
except:
rospy.loginfo("Could not connect to pozyx")
return
# get network id of connected pozyx
self_id = pypozyx.NetworkID()
if pozyx.getNetworkId(self_id) != pypozyx.POZYX_SUCCESS:
rospy.loginfo("Could not get network id")
return
# discover all other pozyx
pozyx.doDiscoveryAll()
devices_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(devices_size)
devices = pypozyx.DeviceList(devices_size)
pozyx.getDeviceIds(devices)
for i in range(devices_size[0]):
rospy.loginfo("Found pozyx device with the id {}".format(
pypozyx.NetworkID(devices[i])))
# publish data
while not rospy.is_shutdown():
for i in range(devices_size[0]):
device_range = pypozyx.DeviceRange()
remote_id = pypozyx.NetworkID(devices[i])
if pozyx.doRanging(self_id[0], device_range, remote_id[0]):
h = Header()
h.stamp = rospy.Time.from_sec(device_range[0] / 1000)
pub.publish(h, remote_id[0], device_range[1], device_range[2])
else:
rospy.loginfo("Error while ranging")
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_device(**kwargs):
p = None
device = get_device()
while True:
device = get_device()
if device:
break
else:
rospy.sleep(1)
rospy.loginfo('Found device %s', device)
while not p and not rospy.is_shutdown():
try:
p = px.PozyxSerial(device, **kwargs)
except SystemExit:
rospy.sleep(1)
return p
def pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_pub')
try:
pozyx = pypozyx.PozyxSerial(str(comports()[0]).split(' ')[0])
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(str(device_range))
else:
rospy.loginfo('ERROR: RANGING')
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 range_publisher():
global msg, device_range, range_pub
try:
range_pub = rospy.Publisher('pozyx/range', Range, queue_size=1)
serial_port = rospy.get_param("port",
pypozyx.get_serial_ports()[0].device)
local_device = pypozyx.PozyxSerial(serial_port)
remote = rospy.get_param("remote", None) # From where, None is local
destination = rospy.get_param("destination", 0x6042) # To where
except:
rospy.loginfo("Serial device Pozyx not found")
return
while not rospy.is_shutdown():
# doRanging : Destination, Device_range, Remote
local_device.doRanging(destination, device_range, remote)
msg.range = device_range.distance
range_pub.publish(msg)
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 pozyx_ranging_pub():
pub = rospy.Publisher('pozyx_device_range', DeviceRange, queue_size=100)
rospy.init_node('range_info_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():
device_range = pypozyx.DeviceRange()
if pozyx.doRanging(destination_id, device_range, remote_id=remote_id):
pub.publish(device_range.timestamp, device_range.distance,
device_range.RSS)
rospy.loginfo(device_range)
else:
rospy.loginfo('ERROR: RANGING')
def set_same_uwb_settings():
rospy.init_node('uwb_configurator')
devices_met = []
uwb_registers = [0x1C, 0x1D, 0x1E, 0x1F]
s = ''
for device in device_ids:
s += '0x%0.4x ' % device
rospy.loginfo("Setting devices with IDs:%s to UWB settings: %s" %
(s, str(new_uwb_settings)))
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
for channel in pypozyx.POZYX_ALL_CHANNELS:
for bitrate in pypozyx.POZYX_ALL_BITRATES:
for prf in pypozyx.POZYX_ALL_PRFS:
for plen in pypozyx.POZYX_ALL_PLENS:
uwb_settings = pypozyx.UWBSettings(
channel, bitrate, prf, plen, gain_db)
pozyx.clearDevices()
pozyx.setUWBSettings(uwb_settings)
pozyx.doDiscovery(pypozyx.POZYX_DISCOVERY_ALL_DEVICES)
device_list_size = pypozyx.SingleRegister()
pozyx.getDeviceListSize(device_list_size)
if device_list_size[0] > 0:
device_list = pypozyx.DeviceList(
list_size=device_list_size[0])
pozyx.getDeviceIds(device_list)
for device in device_list:
if device not in devices_met and device in device_ids:
pozyx.setUWBSettings(new_uwb_settings, device)
pozyx.saveRegisters(uwb_registers, device)
rospy.loginfo(
'Device with ID 0x%0.4x is set' % device)
devices_met.append(device)
pozyx.setUWBSettings(new_uwb_settings)
pozyx.saveRegisters(uwb_registers)
rospy.loginfo("Local device set! Shutting down configurator node now...")
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 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 set_anchor_configuration(port):
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(port)
except:
rospy.loginfo("Pozyx not connected")
return
for tag in tag_ids:
for anchor in anchors:
pozyx.addDevice(anchor, tag)
if len(anchors) > 4:
pozyx.setSelectionOfAnchors(pypozyx.POZYX_ANCHOR_SEL_AUTO,
len(anchors), remote_id=tag)
pozyx.saveRegisters(settings_registers, remote_id=tag)
pozyx.saveNetwork(remote_id=tag)
if tag is None:
rospy.loginfo("Local device configured")
else:
rospy.loginfo("Device with ID 0x%0.4x configured." % tag)
rospy.loginfo("Configuration completed! Shutting down node now...")
def pozyx_positioning_pub():
# pub = rospy.Publisher('pozyx_positioning', Point32, queue_size=100)
pub = rospy.Publisher('/tag_pose0', Point, queue_size=100)
pub1 = rospy.Publisher('/tag_pose1', Point, queue_size=100)
rospy.init_node('positioning_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 = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
rospy.loginfo(coords)
#pub.publish(Point32(coords.x, coords.y, coords.z))
#pub.publish(Point(coords.x, coords.y, coords.z))
pub.publish(
Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0))
pub1.publish(
Point(coords.x / 1000.0, coords.y / 1000.0, coords.z / 1000.0))
def __init__(self, device):
rospy.init_node('pozyx_driver', anonymous=True)
updater = diagnostic_updater.Updater()
self.ps = deque(maxlen=20)
self.es = deque(maxlen=20)
self.remote_id = rospy.get_param('~remote_id', None)
self.base_frame_id = rospy.get_param('~base_frame_id', 'base_link')
debug = rospy.get_param('~debug', False)
self.enable_orientation = rospy.get_param('~enable_orientation', True)
self.enable_position = rospy.get_param('~enable_position', True)
self.enable_raw_sensors = rospy.get_param('~enable_sensors', True)
las = rospy.get_param("~linear_acceleration_stddev", 0.0)
avs = rospy.get_param("~angular_velocity_stddev", 0.0)
mfs = rospy.get_param("~magnetic_field_stddev", 0.0)
os = rospy.get_param("~orientation_stddev", 0.0)
ps = rospy.get_param('~position_stddev', 0.0)
self.pose_cov = cov([ps] * 3 + [os] * 3)
self.orientation_cov = cov([os] * 3)
self.angular_velocity_cov = cov([avs] * 3)
self.magnetic_field_cov = cov([mfs] * 3)
self.linear_acceleration_cov = cov([las] * 3)
_a = rospy.get_param('~algorithm', 'uwb_only')
_d = rospy.get_param('~dimension', '3d')
rate = rospy.get_param('~rate', 1.0) # Hz
self.algorithm = _algorithms.get(_a, px.POZYX_POS_ALG_UWB_ONLY)
self.dimension = _dimensions.get(_d, px.POZYX_3D)
baudrate = rospy.get_param('~baudrate', 115200)
# height of device, required in 2.5D positioning
self.height = rospy.get_param('~height', 0.0) # mm
p = None
while not p and not rospy.is_shutdown():
try:
p = px.PozyxSerial(device,
baudrate=baudrate,
print_output=debug)
except SystemExit:
rospy.sleep(1)
if not p:
return
self.pozyx = PozyxProxy(p, self.remote_id)
self.pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1)
self.pose_cov_pub = rospy.Publisher('pose_cov',
PoseWithCovarianceStamped,
queue_size=1)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.mag_pub = rospy.Publisher('mag', MagneticField, queue_size=1)
self.frame_id = rospy.get_param('~anchors/frame_id')
anchors = []
if rospy.has_param('~anchors/positions'):
anchors_data = rospy.get_param('~anchors/positions')
anchors = [
px.DeviceCoordinates(dev_id, 1, px.Coordinates(x, y, z))
for [dev_id, x, y, z] in anchors_data
]
self.check_visible_devices()
rospy.sleep(0.1)
self.set_anchors(anchors)
rospy.sleep(0.1)
self.check_config(anchors)
rospy.sleep(0.1)
self.tfBuffer = tf2_ros.Buffer()
self.tf = tf2_ros.TransformListener(self.tfBuffer)
if self.enable_orientation:
try:
t = self.tfBuffer.lookup_transform(self.frame_id, 'utm',
rospy.Time(),
rospy.Duration(5.0))
# r = t.transform.rotation
# self.rotation = [r.x, r.y, r.z, r.w]
self.rotation = quaternion_from_euler(0, 0, 0) # = r
# self.rotation = quaternion_multiply(r, self.rotation)
rospy.loginfo('rotation map-> pozyx %s', self.rotation)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logerr('Can not tranform from utm to map frame')
self.enable_orientation = False
sr = px.SingleRegister()
self.pozyx.setOperationMode(0, self.remote_id) # work as a tag
self.pozyx.getWhoAmI(sr, self.remote_id)
_id = '0x%.2x' % sr.data[0]
rospy.set_param('~id', _id)
updater.setHardwareID('Pozyx %s' % _id)
self.pozyx.getFirmwareVersion(sr, self.remote_id)
rospy.set_param('~firmware', register2version(sr))
self.pozyx.getHardwareVersion(sr, self.remote_id)
rospy.set_param('~hardware', register2version(sr))
ni = px.NetworkID()
self.pozyx.getNetworkId(ni)
rospy.set_param('~uwb/network_id', str(ni))
s = px.UWBSettings()
self.pozyx.getUWBSettings(s, self.remote_id)
rospy.set_param('~uwb/channel', s.channel)
rospy.set_param('~uwb/bitrate', s.parse_bitrate())
rospy.set_param('~uwb/prf', s.parse_prf())
rospy.set_param('~uwb/plen', s.parse_plen())
rospy.set_param('~uwb/gain', s.gain_db)
self.pozyx.getOperationMode(sr, self.remote_id)
rospy.set_param('~uwb/mode', 'anchor' if
(sr.data[0] & 1) == 1 else 'tag')
self.pozyx.getSensorMode(sr, self.remote_id)
rospy.set_param('~sensor_mode', sensor_mode(sr))
self_test = self.check_self_test()
if not all(self_test.values()):
rospy.logwarn('Failed Self Test %s', self_test)
else:
rospy.loginfo('Passed Self Test %s', self_test)
freq_bounds = {'min': rate * 0.8, 'max': rate * 1.2}
freq_param = diagnostic_updater.FrequencyStatusParam(
freq_bounds, 0.1, 10)
stamp_param = diagnostic_updater.TimeStampStatusParam()
self.pose_pub_stat = diagnostic_updater.DiagnosedPublisher(
self.pose_pub, updater, freq_param, stamp_param)
updater.add("Sensor calibration", self.update_sensor_diagnostic)
updater.add("Localization", self.update_localization_diagnostic)
rospy.on_shutdown(self.cleanup)
continuous = rospy.get_param('~continuous', False)
rospy.Timer(rospy.Duration(1), lambda evt: updater.update())
rospy.Subscriber('set_anchors', Anchors, self.has_updated_anchors)
if continuous:
if self.enable_position:
ms = int(1000.0 / rate)
# self.pozyx.setUpdateInterval(200)
msr = px.SingleRegister(ms, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr,
self.remote_id)
self.pozyx.setPositionAlgorithm(self.algorithm, self.dimension)
rospy.sleep(0.1)
else:
msr = px.SingleRegister(0, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr,
self.remote_id)
if self.enable_position or self.enable_raw_sensors:
Updater(self, ms * 0.001)
# PositionUpdater(self, ms / 1000.0)
# if self.enable_raw_sensors:
# IMUUpdater(self)
# # position = px.Coordinates()
# # while not rospy.is_shutdown():
# # try:
# # self.pozyx.checkForFlag(bm.POZYX_INT_STATUS_POS, 0.2)
# # self.pozyx.getCoordinates(position)
# # x = np.array([position.x, position.y, position.z])/1000.0
# # self.publish_pose(x)
# # except PozyxException as e:
# # rospy.logerr(e.message)
# # rospy.sleep(1)
rospy.spin()
else:
rospy.Timer(rospy.Duration(1 / rate), self.update)
rospy.spin()
try:
odom_data = com.rxData()
pub.publish(odom_data)
except Exception as e:
rospy.logwarn(e)
pass
com.txData()
if __name__ == "__main__":
rospy.init_node('uwb_node')
serial_port = str(
rospy.get_param('~serial_port', pzx.get_first_pozyx_serial_port()))
frequency = float(rospy.get_param('~frequency', 10))
rate = rospy.Rate(frequency)
pozyx = pzx.PozyxSerial(serial_port)
destination = rospy.get_param('~destination', 0x6e2f)
tx_topic = str(rospy.get_param('~tx_topic', 'uwb_server_tx'))
rx_topic = str(rospy.get_param('~rx_topic', 'uwb_server_rx'))
pub = rospy.Publisher(rx_topic, Odometry, queue_size=10)
com = Communicate(pozyx, destination)
rospy.Subscriber(tx_topic, Odometry, com.odomData)
while not rospy.is_shutdown():
main()
rate.sleep()
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)
self.smoothData(pos)
self.tb.sendTransform(self.pos,
self.quat,
rospy.Time.now(),
"pozyx",
"map"
)
if __name__ == '__main__':
rospy.init_node('pozyx_pose_node')
port = '/dev/ttyACM1'
#port = pypozyx.get_serial_ports()[0].device
# necessary data for calibration, change the IDs and coordinates yourself
anchors = [pypozyx.DeviceCoordinates(0x6e49, 1, pypozyx.Coordinates(0, 0, 590)),
pypozyx.DeviceCoordinates(0x6e57, 1, pypozyx.Coordinates(2900, 400, 720)),
pypozyx.DeviceCoordinates(0x6e32, 1, pypozyx.Coordinates(100, 3000, 1210)),
pypozyx.DeviceCoordinates(0x6e0f, 1, pypozyx.Coordinates(5500, 3200, 910))]
algorithm = pypozyx.POZYX_POS_ALG_UWB_ONLY # positioning algorithm to use
dimension = pypozyx.POZYX_3D # positioning dimension
height = 100 # height of device, required in 2.5D positioning
pozyx = pypozyx.PozyxSerial(port)
remote_id = None #"0x6e3a"
r = ROSPozyx(pozyx, anchors, algorithm, dimension, height,remote_id)
try:
r.pozyx_pose_pub()
except rospy.ROSInterruptException:
pass
def __init__(self, i2c=False, bus=1, port=None):
super().__init__()
self._sensor = pypozyx.PozyxI2C(bus) if i2c else \
pypozyx.PozyxSerial(port or pypozyx.get_first_pozyx_serial_port())
