change all logging to logdatageneric topics

crazyflie/scripts/crazyflie_server.py

@@ -22,6 +22,7 @@
 from crazyflie_interfaces.srv import UploadTrajectory, StartTrajectory, NotifySetpointsStop
 from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult, ParameterType
 from crazyflie_interfaces.msg import Hover
+from crazyflie_interfaces.msg import LogDataGeneric
 
 from std_srvs.srv import Empty
 from geometry_msgs.msg import Twist
@@ -30,21 +31,22 @@
 from sensor_msgs.msg import LaserScan
 from nav_msgs.msg import Odometry
 
+
 import tf_transformations
 from tf2_ros import TransformBroadcaster
 
 from functools import partial
 from math import degrees, radians, pi, cos, sin
 
-cf_log_to_ros_topic = {
- "uint8_t": UInt8,
- "uint16_t": UInt16,
- "uint32_t": UInt32,
- "int8_t": Int8,
- "int16_t": Int16,
- "int32_t": Int32,
- "float": Float32,
-}
+# cf_log_to_ros_topic = {
+#  "uint8_t": UInt8,
+#  "uint16_t": UInt16,
+#  "uint32_t": UInt32,
+#  "int8_t": Int8,
+#  "int16_t": Int16,
+#  "int32_t": Int32,
+#  "float": Float32,
+# }
 
 cf_log_to_ros_param = {
  "uint8_t": ParameterType.PARAMETER_INTEGER,
@@ -192,7 +194,7 @@ def __init__(self):
  for log_name in custom_log_topics[log_group_name]["vars"]:
  lg_custom.add_variable(log_name)
  # Don't know which type this needs to be in until we get the full toc
-  self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_name] = "empty publisher"
+ self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = "empty publisher"
  self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name] = {
  }
  self.swarm._cfs[link_uri].logging["custom_log_groups"][log_group_name]["log_config"] = lg_custom
@@ -340,7 +342,7 @@ def _init_logging(self):
  cf_handle = self.swarm._cfs[link_uri]
  cf = cf_handle.cf
 
- # Start logging for Pose
+ # Start logging for predefined logging
  for default_log_name in self.default_log_type:
  prefix = default_log_name
  if cf_handle.logging[prefix + "_logging_enabled"] and cf_handle.logging["enabled"]:
@@ -350,14 +352,15 @@ def _init_logging(self):
  cf_handle.l_toc = cf.log.toc.toc
  if len(cf_handle.logging["custom_log_groups"]) != 0 and cf_handle.logging["enabled"]:
 
- for log_name in cf_handle.logging["custom_log_publisher"]:
+ #for log_name in cf_handle.logging["custom_log_publisher"]:
+
+ # log_type = cf.log.toc.toc[log_name.split(
+ # '.')[0]][log_name.split('.')[1]].ctype
 
- log_type = cf.log.toc.toc[log_name.split(
- '.')[0]][log_name.split('.')[1]].ctype
- self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_name] = self.create_publisher(
- cf_log_to_ros_topic[log_type], self.cf_dict[link_uri] + "/" + log_name.split('.')[0] + "/" + log_name.split('.')[1], 10)
 
  for log_group_name, log_group_dict in cf_handle.logging["custom_log_groups"].items():
+ self.swarm._cfs[link_uri].logging["custom_log_publisher"][log_group_name] = self.create_publisher(
+ LogDataGeneric, self.cf_dict[link_uri] + "/" + log_group_name, 10)
  lg_custom = log_group_dict['log_config']
  try:
  cf.log.add_config(lg_custom)
@@ -366,10 +369,10 @@ def _init_logging(self):
  lg_custom.error_cb.add_callback(
  self._log_error_callback)
  lg_custom.start()
- self.declare_parameter(
- self.cf_dict[link_uri] + "/logs/" + log_group_name + "/frequency/", log_group_dict['frequency'])
- self.declare_parameter(
- self.cf_dict[link_uri] + "/logs/" + log_group_name + "/vars/", log_group_dict['vars'])
+ #self.declare_parameter(
+ # self.cf_dict[link_uri] + "/logs/" + log_group_name + "/frequency/", log_group_dict['frequency'])
+ #self.declare_parameter(
+ # self.cf_dict[link_uri] + "/logs/" + log_group_name + "/vars/", log_group_dict['vars'])
  except KeyError as e:
  self.get_logger().info(f'{link_uri}: Could not start log configuration,'
  '{} not found in TOC'.format(str(e)))
@@ -534,10 +537,11 @@ def _log_custom_data_callback(self, timestamp, data, logconf, uri):
  for log_name in data:
  log_type = self.swarm._cfs[uri].l_toc[log_name.split(
  '.')[0]][log_name.split('.')[1]].ctype
- msg = cf_log_to_ros_topic[log_type]()
- msg.data = data.get(log_name)
- self.swarm._cfs[uri].logging["custom_log_publisher"][log_name].publish(
- msg)
+ msg = LogDataGeneric() #cf_log_to_ros_topic[log_type]()
+ msg.values.append(data.get(log_name))
+
+ self.swarm._cfs[uri].logging["custom_log_publisher"][logconf.name].publish(
+ msg)
 
  def _log_error_callback(self, logconf, msg):
  print('Error when logging %s: %s' % (logconf.name, msg))
@@ -843,10 +847,13 @@ def _add_logging(self, request, response, uri="all"):
  name=topic_name, period_in_ms=1000 / frequency)
  for log_name in variables:
  lg_custom.add_variable(log_name)
- log_type = self.swarm._cfs[uri].cf.log.toc.toc[log_name.split(
- '.')[0]][log_name.split('.')[1]].ctype
- self.swarm._cfs[uri].logging["custom_log_publisher"][log_name] = self.create_publisher(
- cf_log_to_ros_topic[log_type], self.cf_dict[uri] + "/" + log_name.split('.')[0] + "/" + log_name.split('.')[1], 10)
+ #log_type = self.swarm._cfs[uri].cf.log.toc.toc[log_name.split(
+ # '.')[0]][log_name.split('.')[1]].ctype
+ #self.swarm._cfs[uri].logging["custom_log_publisher"][log_name] = self.create_publisher(
+ # cf_log_to_ros_topic[log_type], self.cf_dict[uri] + "/" + log_name.split('.')[0] + "/" + log_name.split('.')[1], 10)
+ self.swarm._cfs[uri].logging["custom_log_publisher"][log_name] = self.create_publisher(
+ LogDataGeneric, self.cf_dict[uri] + "/" + topic_name, 10)
+
  self.swarm._cfs[uri].cf.log.add_config(lg_custom)
 
  lg_custom.data_received_cb.add_callback(