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():
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_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 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_pozyx(self):
if False and rospy.has_param('~gain_db'):
gain = rospy.get_param('~gain_db')
configure.set_gain(self.pozyx, gain, set_all=False)
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)
if self.continuous:
if self.enable_position:
ms = int(1000.0 / self.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)
else:
msr = px.SingleRegister(0, size=2)
self.pozyx.pozyx.setWrite(rg.POZYX_POS_INTERVAL, msr, self.remote_id)
rospy.sleep(0.1)
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 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 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 run(self):
position = pypozyx.Coordinates()
while position.x == position.y == position.z == 0:
self._sensor.doPositioning(position)
return [(self.sid(), time.time_ns(),
OrderedDict([
("pos_x", position.x),
("pos_y", position.y),
("pos_z", position.z),
]))]
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 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 check_config(self, anchors):
list_size = px.SingleRegister()
self.pozyx.getDeviceListSize(list_size, self.remote_id)
if list_size[0] != len(anchors):
rospy.logerr('anchors were not properly configured')
return
device_list = px.DeviceList(list_size=list_size[0])
self.pozyx.getDeviceIds(device_list, self.remote_id)
for anchor in device_list:
anchor_coordinates = px.Coordinates()
self.pozyx.getDeviceCoordinates(
anchor, anchor_coordinates, self.remote_id)
rospy.loginfo("anchor 0x%0.4x set to %s",
anchor, anchor_coordinates)
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_positioning_pub(r):
mavlink_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=5)
rospy.init_node('positioning_pub', anonymous=True)
#Subscribe to get altitude
rospy.Subscriber("/mavros/imu/static_pressure", FluidPressure, vfr_callback)
#Subscribe to get attitude
rospy.Subscriber("/mavros/imu/data", Imu, imu_data_callback)
f = filo()
mav = MAV_APM.MAVLink(f, srcSystem=1, srcComponent=1)
rospy.sleep(1) # let data show up in subs
pose = types.SimpleNamespace()
while mavlink_pub.get_num_connections() <= 0:
pass
try:
r.loop()
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
r = R.from_quat([
imu_data.orientation.w,
imu_data.orientation.x,
imu_data.orientation.y,
imu_data.orientation.z])
attitude_rad = r.as_euler('xyz', degrees=False)
pose.yaw = attitude_rad[0]
pose.yaw += np.pi*3/2
if pose.yaw>np.pi:
pose.yaw -=2*np.pi
pose.pitch = attitude_rad[1]
pose.roll = attitude_rad[2]
if not (coords.x == 0 and coords.y==0):
temp_x = coords.x/1000
temp_y = coords.y/1000
pose.e = (temp_x*np.cos(3.7)+temp_y*np.sin(3.7))
pose.n = -(-temp_x*np.cos(3.7)+temp_y*np.sin(3.7))
pose.z = -altitude
# print("n: ", pose.n, "e: ", pose.e, "z: ", pose.z, "yaw: ", pose.yaw)
send_vision_position_estimate(mav, mavlink_pub, pose)
def has_updated_anchors(self, msg):
self.frame_id = msg.header.frame_id
anchors = [
px.DeviceCoordinates(
anchor.id, 1,
px.Coordinates(anchor.position.x, anchor.position.y,
anchor.position.z)) for anchor in msg.anchors
]
self.pozyx.lock.acquire(True)
try:
self.set_anchors(anchors)
self.check_config(anchors)
except PozyxException as e:
rospy.logerr(e.message)
finally:
self.pozyx.lock.release()
def getPosition(self, tag_id):
"""
Gets the position of the given tag
Parameters:
tag_id (number): the id of the tag whose position to get
Returns:
(Coordinates): the position of the tag
"""
pos = pypozyx.Coordinates()
status = self.local.doPositioning(pos, remote_id=tag_id)
self._check_status("do positioning {:02x}".format(tag_id), status)
return pos
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 has_updated_anchors(self, msg):
if not self.ready:
return
self.frame_id = msg.header.frame_id
anchors = [
px.DeviceCoordinates(anchor.id, 1, px.Coordinates(
anchor.position.x * 1000.0, anchor.position.y * 1000.0,
anchor.position.z * 1000.0))
for anchor in msg.anchors]
self.pozyx.lock.acquire(True)
try:
self.set_anchors(anchors)
self.check_config(anchors)
except PozyxException as e:
log_pozyx_exception(e)
self.reset_or_exit()
finally:
self.pozyx.lock.release()
def init():
global anchors, tags_id
rospy.init_node('pozyx_configure')
rospy.loginfo("Configuring device list.")
tags_param = rospy.get_param("tags/addresses", [])
tags_id = [None] + tags_param
anchors_param = rospy.get_param("anchors", [])
anchors = []
rospy.loginfo(str(anchors_param))
rospy.loginfo(str(tags_param))
for anchor in anchors_param:
rospy.loginfo(str(anchor))
rospy.loginfo(str(anchor["address"]))
anchors.append(
pypozyx.DeviceCoordinates(
anchor["address"], anchor["flag"],
pypozyx.Coordinates(anchor["position"][0],
anchor["position"][1],
anchor["position"][2])))
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_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_positioning_pub(r):
pub = rospy.Publisher('mavros/local_position/pose',
PoseStamped,
queue_size=100)
rospy.init_node('positioning_pub', anonymous=True)
rate = rospy.Rate(10)
try:
r.loop()
except:
rospy.loginfo("Pozyx not connected")
return
while not rospy.is_shutdown():
pose = PoseStamped()
pose.header = Header()
pose.header.stamp.secs = int(time())
coords = pypozyx.Coordinates()
pozyx.doPositioning(coords, remote_id=remote_id)
rospy.loginfo(coords)
pose.pose.position.x = coords.x
pose.pose.position.y = coords.y
pose.pose.position.z = coords.z
pub.publish(pose)
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 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))
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
This is intended to be highly customisable, but will also be made with parameters and a launch
file in the future. Again, help/suggestions/feedback on these things are highly appreciated.
Run this after running the UWB configuration.
Automatic calibration at this point in time is highly discouraged.
"""
import pypozyx
import rospy
# adding None will cause the local device to be configured for the anchors as well.
#tag_ids = [None, 0x6955, 0x6940, 0x0003, 0x0004]
#tag_ids = [None,0x6955, 0x6940, 0x6937, 0x6935]
tag_ids = [None]
anchors = [
pypozyx.DeviceCoordinates(0x6955, 1, pypozyx.Coordinates(0, 0, 1000)),
pypozyx.DeviceCoordinates(0x6940, 1, pypozyx.Coordinates(5000, 0, 1000)),
pypozyx.DeviceCoordinates(0x6937, 1, pypozyx.Coordinates(0, 5000, 1000)),
pypozyx.DeviceCoordinates(0x6935, 1, pypozyx.Coordinates(5000, 5000, 1000))
]
def set_anchor_configuration():
rospy.init_node('uwb_configurator')
rospy.loginfo("Configuring device list.")
settings_registers = [0x16, 0x17] # POS ALG and NUM ANCHORS
try:
pozyx = pypozyx.PozyxSerial(pypozyx.get_serial_ports()[0].device)
except:
rospy.loginfo("Pozyx not connected")
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)
This is intended to be highly customisable, but will also be made with parameters and a launch
file in the future. Again, help/suggestions/feedback on these things are highly appreciated.
Run this after running the UWB configuration.
Automatic calibration at this point in time is highly discouraged.
"""
import pypozyx
import rospy
from serial.tools.list_ports import comports
# adding None will cause the local device to be configured for the anchors as well.
tag_ids = [None, 0x0001, 0x0002, 0x0003, 0x0004]
anchors = [
pypozyx.DeviceCoordinates(0x0001, 1, pypozyx.Coordinates(0, 0, 5000)),
pypozyx.DeviceCoordinates(0x0002, 1, pypozyx.Coordinates(5000, 0, 1000)),
pypozyx.DeviceCoordinates(0x0003, 1, pypozyx.Coordinates(0, 5000, 1000)),
pypozyx.DeviceCoordinates(0x0004, 1, pypozyx.Coordinates(5000, 5000, 1000))
]
def set_anchor_configuration():
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")
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()
