diff --git a/crazyflie/deps/crazyflie_tools b/crazyflie/deps/crazyflie_tools index 8cf7ee6b0..36ea3d401 160000 --- a/crazyflie/deps/crazyflie_tools +++ b/crazyflie/deps/crazyflie_tools @@ -1 +1 @@ -Subproject commit 8cf7ee6b0e91b78e428e167914c7a9a267e11ac2 +Subproject commit 36ea3d40161db8f580dbb8846789bc036a2ccedd diff --git a/crazyflie/src/crazyflie_server.cpp b/crazyflie/src/crazyflie_server.cpp index c49ac63c4..d79e307f7 100644 --- a/crazyflie/src/crazyflie_server.cpp +++ b/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,21 @@ class CrazyflieROS uint16_t right; } __attribute__((packed)); + struct logStatus { + // general status + uint16_t supervisorInfo; // supervisor.info + // battery related + // Note that using BQ-deck/Bolt one can actually have two batteries at the same time. + // vbat refers to the battery directly connected to the CF board and might not reflect + // the "external" battery on BQ/Bolt builds + 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 +126,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 +138,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 +373,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(); + RCLCPP_INFO(logger_, "Logging to /status at %d Hz", freq); + + publisher_status_ = node->create_publisher(name + "/status", 10); + + std::function cb = std::bind(&CrazyflieROS::on_logging_status, this, std::placeholders::_1, std::placeholders::_2); + + std::list > 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( + &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 +749,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::max(); + } + if (deltaRxUc < 0) { + deltaRxUc += std::numeric_limits::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* values, void* userData) { auto pub = reinterpret_cast::SharedPtr*>(userData); @@ -774,6 +878,15 @@ class CrazyflieROS std::unique_ptr> log_block_scan_; rclcpp::Publisher::SharedPtr publisher_scan_; + std::unique_ptr> log_block_status_; + bool status_has_radio_stats_; + rclcpp::Publisher::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> log_blocks_generic_; std::list::SharedPtr> publishers_generic_; @@ -888,19 +1001,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(); + auto broadcastUri = Crazyflie::broadcastUriFromUnicastUri(uri); + if (broadcaster_.count(broadcastUri) == 0) { + broadcaster_.emplace(broadcastUri, std::make_unique(broadcastUri)); + } + crazyflies_.emplace(name, std::make_unique( 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(broadcastUri)); - } + callback_group_cf_srv_, + broadcaster_.at(broadcastUri).get())); update_name_to_id_map(name, crazyflies_[name]->id()); } diff --git a/crazyflie_interfaces/CMakeLists.txt b/crazyflie_interfaces/CMakeLists.txt index 583e95fa1..0e92f4d09 100644 --- a/crazyflie_interfaces/CMakeLists.txt +++ b/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" diff --git a/crazyflie_interfaces/msg/Status.msg b/crazyflie_interfaces/msg/Status.msg new file mode 100644 index 000000000..bebd42a71 --- /dev/null +++ b/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