-
Notifications
You must be signed in to change notification settings - Fork 3
Ad Hoc Log Configurations
This section concerns how to create special configurations of telemetry (such as sensor data or controller and estimator state) that are not covered by defined ROS messages. The Crazyflie records many variables, and the logging system of the crazyflie allows any subset of those variables (limited to 24 bytes) to be transmitted via the radio at regular intervals.
To create an ad hoc log configuration, you must
- provide a name for the configuration
- specify the log variables that the crazyflie will transmit
- provide the transmission period
- provide a callback method to receive the data
Log configurations can be created by using either the CrazyflieClient object, or by using the AddLogConfig action server. Both ways require building a list of LogVariables.
LogVariable is a class in the crazyflie_client submodle. Its constructor takes two arguments:
- variable_name (type string)
- variable_type (type string)
The variable name must match the name of a variable in the Crazyflie log table of contents. Variable names have two parts; the log group, and the log variable. For example, the variable name of the roll variable in the stabilizer group is 'stabilizer.roll'. The variable type must be equal to one of the following values:
Name | Size (bytes) | Description |
---|---|---|
'float' | 4 | Single precision float |
'FP16' | 2 | Half precision float |
'int8_t' | 1 | 8-bit signed integer |
'int16_t' | 2 | 16-bit signed integer |
int32_t' | 4 | 32-bit signed integer |
uint8_t' | 1 | 8-bit unsigned integer (byte or char) |
'uint16_t' | 2 | 16-bit unsigned integer |
'uint32_t' | 4 | 32-bit unsigned integer |
Here is an example that creates a custom log configuration which consists of data from groups 'stabilizer' and 'kalman'.
# Necessary imports
import rospy
from rospy_crazyflie.crazyflie_client import CrazyflieClient
from rospy_crazyflie.msgs import LogVariable
rospy.init_node('example')
client = CrazyflieClient('/crazyflie_server/crazyflie1')
# Define the callback function
# @data dictionary associating variable names with values
# @timestamp crazyflie system tick count at time the log was sent
def log_callback(data, timestamp):
print("data: {}".format(data))
print("timestamp: {}".format(timestamp))
# Creating the log config
config_name = 'custom_log_config'
config_vars = [
LogVariable('stabilizer.roll', 'float'),
LogVariable('stabilizer.pitch', 'float'),
LogVariable('stabilizer.yaw', 'float'),
LogVariable('kalman.stateX', 'float'),
LogVariable('kalman.stateY', 'float'),
LogVariable('kalman.stateZ', 'float')
]
period_in_ms = 100 # sends this log 10 times per second
client.add_log_config(config_name, config_vars, period_in_ms, callback=log_callback)
You can add log configs directly through the server node by using ROS actionlib. The log data will be published to a LogData topic at <server namespace>/<config name>
. Note that LogData contains a JSON serialization of the log contents.
# Necessary imports
import rospy
import actionlib
from rospy_crazyflie.msg import LogConfig, LogVariable, AddLogConfigAction, AddLogConfigGoal
rospy.init_node('example')
# Get a simple action client
add_log_config_client = actionlib.SimpleActionClient(
'/crazyflie_server/add_log_config',
AddLogConfigAction
)
# Assemble the goal
config_name = 'custom_log_config'
config_vars = [
LogVariable('stabilizer.roll', 'float'),
LogVariable('stabilizer.pitch', 'float'),
LogVariable('stabilizer.yaw', 'float'),
LogVariable('kalman.stateX', 'float'),
LogVariable('kalman.stateY', 'float'),
LogVariable('kalman.stateZ', 'float')
]
period_in_ms = 100
config = LogConfig(config_name, config_vars, period_in_ms)
add_log_config_client.send_goal(AddLogConfigGoal(config))