def _cb(self, msg):
influx_time = timestamp_to_influxdb_time(msg.header.stamp)
joint_names = msg.name
position = msg.position
velocity = msg.velocity
effort = msg.effort
with self.lock:
self.query.append({
"measurement": "joint_states",
"tags": {
"type": "position"
},
"time": influx_time,
"fields": dict(zip(joint_names, position))
})
self.query.append({
"measurement": "joint_states",
"tags": {
"type": "velocity"
},
"time": influx_time,
"fields": dict(zip(joint_names, velocity))
})
self.query.append({
"measurement": "joint_states",
"tags": {
"type": "effort"
},
"time": influx_time,
"fields": dict(zip(joint_names, effort))
})
def _cb(self, msg):
time = timestamp_to_influxdb_time(msg.header.stamp)
battery_id = msg.id
battery0_temp = msg.battery[0].battery_register[8] / 10.0 - 273.15
battery1_temp = msg.battery[1].battery_register[8] / 10.0 - 273.15
battery2_temp = msg.battery[2].battery_register[8] / 10.0 - 273.15
battery3_temp = msg.battery[3].battery_register[8] / 10.0 - 273.15
battery0_voltage = msg.battery[0].battery_register[9] / 1000.0
battery1_voltage = msg.battery[1].battery_register[9] / 1000.0
battery2_voltage = msg.battery[2].battery_register[9] / 1000.0
battery3_voltage = msg.battery[3].battery_register[9] / 1000.0
battery0_current = msg.battery[0].battery_register[11] / 1000.0
battery1_current = msg.battery[1].battery_register[11] / 1000.0
battery2_current = msg.battery[2].battery_register[11] / 1000.0
battery3_current = msg.battery[3].battery_register[11] / 1000.0
battery0_charge = msg.battery[0].battery_register[13]
battery1_charge = msg.battery[1].battery_register[13]
battery2_charge = msg.battery[2].battery_register[13]
battery3_charge = msg.battery[3].battery_register[13]
average_temp = (battery0_temp + battery1_temp + battery2_temp +
battery3_temp) / 4.0
average_voltage = (battery0_voltage + battery1_voltage +
battery2_voltage + battery3_voltage) / 4.0
average_current = (battery0_current + battery1_current +
battery2_current + battery3_current) / 4.0
average_charge = msg.average_charge / 100.0
query = [{
"measurement": "battery_states",
"tags": {
"battery_id": battery_id
},
"time": time,
"fields": {
"temperature": average_temp,
"voltage": average_voltage,
"current": average_current,
"charge_percent": average_charge,
"battery0_temperature": battery0_temp,
"battery1_temperature": battery1_temp,
"battery2_temperature": battery2_temp,
"battery3_temperature": battery3_temp,
"battery0_voltage": battery0_voltage,
"battery1_voltage": battery1_voltage,
"battery2_voltage": battery2_voltage,
"battery3_voltage": battery3_voltage,
"battery0_current": battery0_current,
"battery1_current": battery1_current,
"battery2_current": battery2_current,
"battery3_current": battery3_current,
"battery0_charge_percent": battery0_charge,
"battery1_charge_percent": battery1_charge,
"battery2_charge_percent": battery2_charge,
"battery3_charge_percent": battery3_charge,
}
}]
if len(query) > 0:
self.client.write_points(query, time_precision='ms')
def _receive_cb(self, msg):
receive_time = timestamp_to_influxdb_time(rospy.Time.now())
receive_bps = msg.data
query = [{
"measurement": "network_states",
"tags": {
"type": "receive"
},
"time": receive_time,
"fields": {
"bps": receive_bps,
}
}]
self.client.write_points(query, time_precision='ms')
def _cb(self, msg):
time = timestamp_to_influxdb_time(rospy.Time.now())
battery_name = msg.name
charge_percent = msg.charge_level
query = [{
"measurement": "battery_states",
"tags": {
"battery_name": battery_name
},
"time": time,
"fields": {
"charge_percent": charge_percent,
}
}]
self.client.write_points(query, time_precision='ms')
def _update_cb(self, event):
# if event.last_real:
# timestamp = event.last_real
# else:
# timestamp = event.current_real - self.update_rate
timestamp = event.current_real
influx_time = timestamp_to_influxdb_time(timestamp)
trans_x_fields = {}
trans_y_fields = {}
trans_z_fields = {}
rot_x_fields = {}
rot_y_fields = {}
rot_z_fields = {}
rot_w_fields = {}
rot_theta_fields = {}
with self.graph_lock:
graph = copy.deepcopy(self.graph)
for child_frame_id, _ in graph.items():
try:
transform_stamped = self.tf_buffer.lookup_transform(
self.frame_id, child_frame_id, rospy.Time(0))
except tf2_ros.ExtrapolationException as e:
rospy.logerr_throttle(
60.0, 'tf2_ros.ExtrapolationException: {}'.format(e))
continue
except tf2_ros.ConnectivityException as e:
rospy.logerr_throttle(
60.0, 'tf2_ros.ConnectivityException: {}'.format(e))
continue
except tf2_ros.LookupException as e:
rospy.logerr_throttle(60.0,
'tf2_ros.LookupException: {}'.format(e))
continue
translation = transform_stamped.transform.translation
rotation = transform_stamped.transform.rotation
theta = 2 * np.arctan(
np.linalg.norm([rotation.x, rotation.y, rotation.z]) /
rotation.w)
trans_x_fields[child_frame_id] = translation.x
trans_y_fields[child_frame_id] = translation.y
trans_z_fields[child_frame_id] = translation.z
rot_x_fields[child_frame_id] = rotation.x
rot_y_fields[child_frame_id] = rotation.y
rot_z_fields[child_frame_id] = rotation.z
rot_w_fields[child_frame_id] = rotation.w
rot_theta_fields[child_frame_id] = theta
with self.lock:
if len(trans_x_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "translation",
"field": "x",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": trans_x_fields
})
if len(trans_y_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "translation",
"field": "y",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": trans_y_fields
})
if len(trans_z_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "translation",
"field": "z",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": trans_z_fields
})
if len(rot_x_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "rotation",
"field": "x",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": rot_x_fields
})
if len(rot_y_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "rotation",
"field": "y",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": rot_y_fields
})
if len(rot_z_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "rotation",
"field": "z",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": rot_z_fields
})
if len(rot_w_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "rotation",
"field": "w",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": rot_w_fields
})
if len(rot_theta_fields) > 0:
self.query.append({
"measurement": self.measurement_name,
"tags": {
"type": "rotation",
"field": "theta",
"frame_id": self.frame_id
},
"time": influx_time,
"fields": rot_theta_fields
})