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()