def rxData(self):
self.pozyx.getRxInfo(
self.rx_info) # Get the length of the received data
data = Data(
[0] * self.rx_info[1]) # Create a buffer for the incomming string
self.pozyx.readRXBufferData(
data) # Read the received data from the registery
message = str()
for i in data:
message = message + chr(i)
s = zlib.decompress(message) # Decompress zlib string
y = json.loads(s) # Deserialize the JSON string
odom_data_pub = Odometry()
odom_data_pub.pose.pose.position.x = y['a']
odom_data_pub.pose.pose.position.y = y['b']
odom_data_pub.pose.pose.orientation.z = y['c']
odom_data_pub.pose.pose.orientation.w = y['d']
odom_data_pub.twist.twist.linear.x = y['e']
odom_data_pub.twist.twist.angular.z = y['f']
self.odom_data_rx = odom_data_pub
def rxData(self):
self.pozyx.getRxInfo(self.rx_info)
data = Data([0]*self.rx_info[1])
self.pozyx.readRXBufferData(data)
message = str()
for i in data:
message = message + chr(i)
s = zlib.decompress(message)
y = json.loads(s)
odom_data_pub = Odometry()
odom_data_pub.header.frame_id = y['a']
odom_data_pub.child_frame_id = y['b']
odom_data_pub.pose.pose.position.x = y['c']
odom_data_pub.pose.pose.position.y = y['d']
odom_data_pub.pose.pose.orientation.z = y['e']
odom_data_pub.pose.pose.orientation.w = y['f']
odom_data_pub.twist.twist.linear.x = y['g']
odom_data_pub.twist.twist.angular.z = y['h']
return odom_data_pub
def send_msp_message(msg):
message = list(msg.values())
size = len(message)
if size > 27:
return {'error': 'message too long!'}
d = Data(data=message, data_format=size * 'B')
pozyx.sendData(destination=remote_id, data=d)
return {'success': 'WP sent'}
def txData(self):
x = {
"a": round(self.odom_data.pose.pose.position.x, 4),
"b": round(self.odom_data.pose.pose.position.y, 4),
"c": round(self.odom_data.pose.pose.orientation.z, 4),
"d": round(self.odom_data.pose.pose.orientation.w, 4),
"e": round(self.odom_data.twist.twist.linear.x, 4),
"f": round(self.odom_data.twist.twist.angular.z, 4)
}
s = json.dumps(x)
comp_data = zlib.compress(str(s))
rospy.loginfo(len(comp_data))
data = Data([ord(c) for c in comp_data])
self.pozyx.sendData(self.destination, data)
def set_device_gain(self, dev_id, gain):
tx_power = Data([0], 'f')
gain_ok = False
retry = 0
while (not gain_ok) and (retry < 5):
if self.pozyx.getTxPower(tx_power, dev_id) == POZYX_SUCCESS:
if tx_power.data != gain:
self.pozyx.setTxPower(CONFIG_TX_GAIN, dev_id)
else:
gain_ok = True
retry += 1
return gain_ok
""" split line """
def txData(self):
x = { # Create the JSON object with current odom data
"a": round(self.odom_data.pose.pose.position.x, 4),
"b": round(self.odom_data.pose.pose.position.y, 4),
"c": round(self.odom_data.pose.pose.orientation.z, 4),
"d": round(self.odom_data.pose.pose.orientation.w, 4),
"e": round(self.odom_data.twist.twist.linear.x, 4),
"f": round(self.odom_data.twist.twist.angular.z, 4)
}
s = json.dumps(x) # Serialize the JSON object
comp_data = zlib.compress(
str(s)
) # Compress the string with zlib to keep the string length < 100 bytes
data = Data([ord(c) for c in comp_data
]) # Create buffer for the compressed string
self.pozyx.sendData(self.destination,
data) # Send the data to the external robot