Add status topic

crazyflie/src/crazyflie_server.cpp

@@ -20,6 +20,7 @@
 #include "motion_capture_tracking_interfaces/msg/named_pose_array.hpp"
 #include "crazyflie_interfaces/msg/full_state.hpp"
 #include "crazyflie_interfaces/msg/position.hpp"
+#include "crazyflie_interfaces/msg/status.hpp"
 #include "crazyflie_interfaces/msg/log_data_generic.hpp"
 #include "crazyflie_interfaces/msg/connection_statistics_array.hpp"
 
@@ -102,6 +103,19 @@ class CrazyflieROS
     uint16_t right;
   } __attribute__((packed));
 
+  struct logStatus {
+    // general status
+    uint16_t supervisorInfo; // supervisor.info
+    // battery related
+    // TODO: would it be better to use pm.batteryLevel?
+    uint16_t vbatMV;  // pm.vbatMV
+    uint8_t pmState;  // pm.state
+    // radio related
+    uint8_t rssi;     // radio.rssi
+    uint16_t numRxBc; // radio.numRxBc
+    uint16_t numRxUc; // radio.numRxUc
+  } __attribute__((packed));
+
 public:
   CrazyflieROS(
     const std::string& link_uri,
@@ -110,6 +124,7 @@ class CrazyflieROS
     rclcpp::Node* node,
     rclcpp::CallbackGroup::SharedPtr callback_group_cf_cmd,
     rclcpp::CallbackGroup::SharedPtr callback_group_cf_srv,
+    const CrazyflieBroadcaster* cfbc,
     bool enable_parameters = true)
     : logger_(rclcpp::get_logger(name))
     , cf_logger_(logger_)
@@ -121,6 +136,7 @@ class CrazyflieROS
     , node_(node)
     , tf_broadcaster_(node)
     , last_on_latency_(std::chrono::steady_clock::now())
+    , cfbc_(cfbc)
   {
     auto sub_opt_cf_cmd = rclcpp::SubscriptionOptions();
     sub_opt_cf_cmd.callback_group = callback_group_cf_cmd;
@@ -355,6 +371,47 @@ class CrazyflieROS
               }, cb));
             log_block_scan_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
           }
+          else if (i.first.find("default_topics.status") == 0) {
+            int freq = log_config_map["default_topics.status.frequency"].get<int>();
+            RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq);
+
+            publisher_status_ = node->create_publisher<crazyflie_interfaces::msg::Status>(name + "/status", 10);
+
+            std::function<void(uint32_t, const logStatus*)> cb = std::bind(&CrazyflieROS::on_logging_status, this, std::placeholders::_1, std::placeholders::_2);
+
+            std::list<std::pair<std::string, std::string> > logvars({
+              // general status
+              {"supervisor", "info"},
+              // battery related
+              {"pm", "vbatMV"},
+              {"pm", "state"},
+              // radio related
+              {"radio", "rssi"}
+            });
+
+            // check if this firmware version has radio.numRx{Bc,Uc}
+            status_has_radio_stats_ = false;
+            for (auto iter = cf_.logVariablesBegin(); iter != cf_.logVariablesEnd(); ++iter) {
+              auto entry = *iter;
+              if (entry.group == "radio" && entry.name == "numRxBc") {
+                logvars.push_back({"radio", "numRxBc"});
+                logvars.push_back({"radio", "numRxUc"});
+                status_has_radio_stats_ = true;
+                break;
+              }
+            }
+
+            // older firmware -> use other 16-bit variables
+            if (!status_has_radio_stats_) {
+                RCLCPP_WARN(logger_, "Older firmware. status/num_rx_broadcast and status/num_rx_unicast are set to zero.");
+                logvars.push_back({"pm", "vbatMV"});
+                logvars.push_back({"pm", "vbatMV"});
+            }
+
+            log_block_status_.reset(new LogBlock<logStatus>(
+              &cf_,logvars, cb));
+            log_block_status_->start(uint8_t(100.0f / (float)freq)); // this is in tens of milliseconds
+          }
           else if (i.first.find("custom_topics") == 0
                    && i.first.rfind(".vars") != std::string::npos) {
             std::string topic_name = i.first.substr(14, i.first.size() - 14 - 5);
@@ -690,6 +747,51 @@ class CrazyflieROS
     }
   }
 
+  void on_logging_status(uint32_t time_in_ms, const logStatus* data) {
+    if (publisher_status_) {
+
+      crazyflie_interfaces::msg::Status msg;
+      msg.header.stamp = node_->get_clock()->now();
+      msg.header.frame_id = name_;
+      msg.supervisor_info = data->supervisorInfo;
+      msg.battery_voltage = data->vbatMV / 1000.0f;
+      msg.pm_state = data->pmState;
+      msg.rssi = data->rssi;
+      if (status_has_radio_stats_) {
+        int32_t deltaRxBc = data->numRxBc - previous_numRxBc;
+        int32_t deltaRxUc = data->numRxUc - previous_numRxUc;
+        // handle overflow
+        if (deltaRxBc < 0) {
+          deltaRxBc += std::numeric_limits<uint16_t>::max();
+        }
+        if (deltaRxUc < 0) {
+          deltaRxUc += std::numeric_limits<uint16_t>::max();
+        }
+        msg.num_rx_broadcast = deltaRxBc;
+        msg.num_rx_unicast = deltaRxUc;
+        previous_numRxBc = data->numRxBc;
+        previous_numRxUc = data->numRxUc;
+      } else {
+        msg.num_rx_broadcast = 0;
+        msg.num_rx_unicast = 0;
+      }
+
+      // connection sent stats (unicast)
+      const auto statsUc = cf_.connectionStats();
+      size_t deltaTxUc = statsUc.sent_count - previous_stats_unicast_.sent_count;
+      msg.num_tx_unicast = deltaTxUc;
+      previous_stats_unicast_ = statsUc;
+
+      // connection sent stats (broadcast)
+      const auto statsBc = cfbc_->connectionStats();
+      size_t deltaTxBc = statsBc.sent_count - previous_stats_broadcast_.sent_count;
+      msg.num_tx_broadcast = deltaTxBc;
+      previous_stats_broadcast_ = statsBc;
+
+      publisher_status_->publish(msg);
+    }
+  }
+
   void on_logging_custom(uint32_t time_in_ms, const std::vector<float>* values, void* userData) {
 
     auto pub = reinterpret_cast<rclcpp::Publisher<crazyflie_interfaces::msg::LogDataGeneric>::SharedPtr*>(userData);
@@ -774,6 +876,15 @@ class CrazyflieROS
   std::unique_ptr<LogBlock<logScan>> log_block_scan_;
   rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr publisher_scan_;
 
+  std::unique_ptr<LogBlock<logStatus>> log_block_status_;
+  bool status_has_radio_stats_;
+  rclcpp::Publisher<crazyflie_interfaces::msg::Status>::SharedPtr publisher_status_;
+  uint16_t previous_numRxBc;
+  uint16_t previous_numRxUc;
+  bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_unicast_;
+  bitcraze::crazyflieLinkCpp::Connection::Statistics previous_stats_broadcast_;
+  const CrazyflieBroadcaster* cfbc_;
+
   std::list<std::unique_ptr<LogBlockGeneric>> log_blocks_generic_;
   std::list<rclcpp::Publisher<crazyflie_interfaces::msg::LogDataGeneric>::SharedPtr> publishers_generic_;
 
@@ -888,19 +999,19 @@ class CrazyflieServer : public rclcpp::Node
         // if it is a Crazyflie, try to connect
         if (constr == "crazyflie") {
           std::string uri = parameter_overrides.at("robots." + name + ".uri").get<std::string>();
+          auto broadcastUri = Crazyflie::broadcastUriFromUnicastUri(uri);
+          if (broadcaster_.count(broadcastUri) == 0) {
+            broadcaster_.emplace(broadcastUri, std::make_unique<CrazyflieBroadcaster>(broadcastUri));
+          }
+
           crazyflies_.emplace(name, std::make_unique<CrazyflieROS>(
             uri,
             cf_type,
             name,
             this,
             callback_group_cf_cmd_,
-            callback_group_cf_srv_));
-
-          auto broadcastUri = crazyflies_[name]->broadcastUri();
-          RCLCPP_INFO(logger_, "%s", broadcastUri.c_str());
-          if (broadcaster_.count(broadcastUri) == 0) {
-            broadcaster_.emplace(broadcastUri, std::make_unique<CrazyflieBroadcaster>(broadcastUri));
-          }
+            callback_group_cf_srv_,
+            broadcaster_.at(broadcastUri).get()));
 
           update_name_to_id_map(name, crazyflies_[name]->id());
         }

crazyflie_interfaces/CMakeLists.txt

@@ -25,6 +25,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Hover.msg"
   "msg/LogBlock.msg"
   "msg/Position.msg"
+  "msg/Status.msg"
   "msg/TrajectoryPolynomialPiece.msg"
   "msg/VelocityWorld.msg"
   "srv/GoTo.srv"

crazyflie_interfaces/msg/Status.msg

@@ -0,0 +1,31 @@
+
+# Constants
+uint16 SUPERVISOR_INFO_CAN_BE_ARMED = 0
+uint16 SUPERVISOR_INFO_IS_ARMED     = 2
+uint16 SUPERVISOR_INFO_AUTO_ARM     = 4
+uint16 SUPERVISOR_INFO_CAN_FLY      = 8
+uint16 SUPERVISOR_INFO_IS_FLYING    = 16
+uint16 SUPERVISOR_INFO_IS_TUMBLED   = 32
+uint16 SUPERVISOR_INFO_IS_LOCKED    = 64
+
+uint8 PM_STATE_BATTERY   = 0
+uint8 PM_STATE_CHARGING  = 1
+uint8 PM_STATE_CHARGED   = 2
+uint8 PM_STATE_LOW_POWER = 3
+uint8 PM_STATE_SHUTDOWN  = 4
+
+std_msgs/Header header
+
+# general status
+uint16 supervisor_info # Bitfield, see SUPERVISOR_INFO for individual bits
+
+# battery related
+float32 battery_voltage  # internal battery voltage [V]
+uint8 pm_state # See PM_STATE_* for potential values
+
+# radio related
+uint8 rssi      # latest radio signal strength indicator [dBm]
+uint32 num_rx_broadcast  # number of received broadcast packets during the last period
+uint32 num_tx_broadcast  # number of broadcast packets sent during the last period
+uint32 num_rx_unicast    # number of received unicast packets during the last period
+uint32 num_tx_unicast    # number of unicast packets sent during the last period