diff --git a/include/control/control.h b/include/control/control.h index ac7f7e3..1c16bda 100644 --- a/include/control/control.h +++ b/include/control/control.h @@ -89,4 +89,4 @@ class ControlTool : public rviz::SelectionTool{ } // namespace mrs_rviz_plugins -#endif \ No newline at end of file +#endif diff --git a/include/control/drone_entity.h b/include/control/drone_entity.h index da8c2c5..407380e 100644 --- a/include/control/drone_entity.h +++ b/include/control/drone_entity.h @@ -41,10 +41,7 @@ class DroneEntity { std::vector getGains(); std::vector getControllers(); std::vector getTrackers(); - std::vector getOdomSources(); - std::vector getLatEstimators(); - std::vector getAltEstimators(); - std::vector getHdgEstimators(); + std::vector getOdomEstimators(); bool getNullTracker(); void setServiceNumCalls(const int value); @@ -126,10 +123,10 @@ class DroneEntity { std::vector controllers{}; std::vector trackers{}; std::vector - odom_lat_sources{}; // Seems to be the same as odom source + odom_estimators{}; // Seems to be the same as odom source // (https://github.com/ctu-mrs/mrs_uav_status/blob/78f9ee8fce216a810d15d14d7a4e479e2c41d503/src/status.cpp#L872C65-L872C65) - std::vector odom_alt_sources{}; - std::vector odom_hdg_sources{}; + /* std::vector odom_alt_sources{}; */ + /* std::vector odom_hdg_sources{}; */ // | ----------------------- Attributes ----------------------- | diff --git a/include/uav_status/status_display.h b/include/uav_status/status_display.h index 6c63cc0..f0ade84 100644 --- a/include/uav_status/status_display.h +++ b/include/uav_status/status_display.h @@ -95,8 +95,8 @@ private Q_SLOTS: std::pair getSpawnCoordinates(rviz::Property* property); std::pair getBottomLine(); // Meant to take "Global Options" property only! - void setTextColor(rviz::Property* property); - bool getIsInited() { + void setTextColor(rviz::Property* property); + bool getIsInited() { return is_inited; } QColor getColor(const int code) { @@ -191,9 +191,7 @@ private Q_SLOTS: double cmd_z = 0; double cmd_hdg = 0; std::string odom_frame = "!NO DATA!"; - std::string curr_estimator_hori = "!NO DATA!"; - std::string curr_estimator_vert = "!NO DATA!"; - std::string curr_estimator_hdg = "!NO DATA!"; + std::string curr_estimator = "!NO DATA!"; bool odom_update_required = true; // General info @@ -205,22 +203,22 @@ private Q_SLOTS: bool comp_state_update_required = true; // Hw api state - double hw_api_rate = 0; - double hw_api_state_rate = 0; - double hw_api_cmd_rate = 0; - double hw_api_battery_rate = 0; - bool hw_api_gnss_ok = false; - bool hw_api_armed = false; - std::string hw_api_mode = ""; - double battery_volt = 0; - double battery_curr = 0; - double battery_wh_drained = 0; - double thrust = 0; - double mass_estimate = 0; - double mass_set = 0; - double hw_api_gnss_qual = 0; - double mag_norm = 0; - double mag_norm_rate = 0; + double hw_api_rate = 0; + double hw_api_state_rate = 0; + double hw_api_cmd_rate = 0; + double hw_api_battery_rate = 0; + bool hw_api_gnss_ok = false; + bool hw_api_armed = false; + std::string hw_api_mode = ""; + double battery_volt = 0; + double battery_curr = 0; + double battery_wh_drained = 0; + double thrust = 0; + double mass_estimate = 0; + double mass_set = 0; + double hw_api_gnss_qual = 0; + double mag_norm = 0; + double mag_norm_rate = 0; bool hw_api_state_update_required = true; // Custom topics @@ -237,14 +235,14 @@ private Q_SLOTS: bool node_stats_update_required = true; // | ----------------------- Attributes ----------------------- | - ros::NodeHandle nh; - ros::Subscriber uav_status_sub; - QColor bg_color = QColor(0, 0, 0, 100); - QColor fg_color = QColor(25, 255, 240, 255); - bool is_inited = false; - static int display_number; - int id; - std::string last_uav_name; + ros::NodeHandle nh; + ros::Subscriber uav_status_sub; + QColor bg_color = QColor(0, 0, 0, 100); + QColor fg_color = QColor(25, 255, 240, 255); + bool is_inited = false; + static int display_number; + int id; + std::string last_uav_name; static std::unordered_map taken_uavs; // | --------------------- Default values --------------------- | diff --git a/src/control/drone_entity.cpp b/src/control/drone_entity.cpp index de0a720..0b74caa 100644 --- a/src/control/drone_entity.cpp +++ b/src/control/drone_entity.cpp @@ -135,9 +135,9 @@ void DroneEntity::updateMenu() { [this, tracker](const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { setTracker(tracker, feedback); }); } - for (const auto odom_source : odom_lat_sources) { - menu_handler->insert(entries[SET_ESTIMATOR], odom_source, - [this, odom_source](const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { setEstimator(odom_source, feedback); }); + for (const auto odom_estimator : odom_estimators) { + menu_handler->insert(entries[SET_ESTIMATOR], odom_estimator, + [this, odom_estimator](const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { setEstimator(odom_estimator, feedback); }); } menu_handler->apply(*server, name + " marker"); @@ -152,9 +152,9 @@ void DroneEntity::statusCallback(const mrs_msgs::UavStatusConstPtr &msg) { updated |= compareAndUpdate(gains, msg->gains); updated |= compareAndUpdate(controllers, msg->controllers); updated |= compareAndUpdate(trackers, msg->trackers); - updated |= compareAndUpdate(odom_lat_sources, msg->odom_estimators_hori); - updated |= compareAndUpdate(odom_alt_sources, msg->odom_estimators_vert); - updated |= compareAndUpdate(odom_hdg_sources, msg->odom_estimators_hdg); + updated |= compareAndUpdate(odom_estimators, msg->odom_estimators); + /* updated |= compareAndUpdate(odom_alt_sources, msg->odom_estimators_vert); */ + /* updated |= compareAndUpdate(odom_hdg_sources, msg->odom_estimators_hdg); */ updated |= msg->null_tracker != null_tracker; null_tracker = msg->null_tracker; @@ -451,20 +451,8 @@ std::vector DroneEntity::getTrackers() { return trackers; } -std::vector DroneEntity::getOdomSources() { - return odom_lat_sources; -} - -std::vector DroneEntity::getLatEstimators() { - return odom_lat_sources; -} - -std::vector DroneEntity::getAltEstimators() { - return odom_alt_sources; -} - -std::vector DroneEntity::getHdgEstimators() { - return odom_hdg_sources; +std::vector DroneEntity::getOdomEstimators() { + return odom_estimators; } bool DroneEntity::getNullTracker() { diff --git a/src/control/im_server.cpp b/src/control/im_server.cpp index 264406e..59e49ee 100644 --- a/src/control/im_server.cpp +++ b/src/control/im_server.cpp @@ -96,17 +96,17 @@ boost::shared_ptr ImServer::getMenu() { // Create basic instances menu.reset(new QMenu()); - QAction* land = new QAction("Land", menu.get()); - QAction* land_home = new QAction("Land Home", menu.get()); - QAction* takeoff = new QAction("Takeoff", menu.get()); - QMenu* set_constraints = new QMenu("Set Constraints", menu.get()); - QMenu* set_gains = new QMenu("Set Gains", menu.get()); - QMenu* set_controller = new QMenu("Set Controller", menu.get()); - QMenu* set_tracker = new QMenu("Set Tracker", menu.get()); - QMenu* set_odom_source = new QMenu("Set Odom Source", menu.get()); - QMenu* set_lat_estimator = new QMenu("Set Lat Estimator", menu.get()); - QMenu* set_alt_estimator = new QMenu("Set Alt Estimator", menu.get()); - QMenu* set_hdg_estimator = new QMenu("Set Hdg Estimator", menu.get()); + QAction* land = new QAction("Land", menu.get()); + QAction* land_home = new QAction("Land Home", menu.get()); + QAction* takeoff = new QAction("Takeoff", menu.get()); + QMenu* set_constraints = new QMenu("Set Constraints", menu.get()); + QMenu* set_gains = new QMenu("Set Gains", menu.get()); + QMenu* set_controller = new QMenu("Set Controller", menu.get()); + QMenu* set_tracker = new QMenu("Set Tracker", menu.get()); + QMenu* set_odom_estimator = new QMenu("Set Odom Estimator", menu.get()); + QMenu* set_lat_estimator = new QMenu("Set Lat Estimator", menu.get()); + QMenu* set_alt_estimator = new QMenu("Set Alt Estimator", menu.get()); + QMenu* set_hdg_estimator = new QMenu("Set Hdg Estimator", menu.get()); // Connect signals connect(land, &QAction::triggered, this, &ImServer::landNow); @@ -129,14 +129,11 @@ boost::shared_ptr ImServer::getMenu() { // Filter the options that are not present in every selected drone DroneEntity* drone = selected_drones[0]; - std::vector constraint_options = drone->getConstraints(); - std::vector gain_options = drone->getGains(); - std::vector controller_options = drone->getControllers(); - std::vector tracker_options = drone->getTrackers(); - std::vector odom_source_options = drone->getOdomSources(); - std::vector odom_lat_estimtor_options = drone->getLatEstimators(); - std::vector odom_alt_estimtor_options = drone->getAltEstimators(); - std::vector odom_hdg_estimtor_options = drone->getHdgEstimators(); + std::vector constraint_options = drone->getConstraints(); + std::vector gain_options = drone->getGains(); + std::vector controller_options = drone->getControllers(); + std::vector tracker_options = drone->getTrackers(); + std::vector odom_estimators_options = drone->getOdomEstimators(); std::vector present_options; for (auto& selected_drone : selected_drones) { @@ -152,17 +149,8 @@ boost::shared_ptr ImServer::getMenu() { present_options = selected_drone->getTrackers(); chooseOptions(tracker_options, present_options); - present_options = selected_drone->getOdomSources(); - chooseOptions(odom_source_options, present_options); - - present_options = selected_drone->getLatEstimators(); - chooseOptions(odom_lat_estimtor_options, present_options); - - present_options = selected_drone->getAltEstimators(); - chooseOptions(odom_alt_estimtor_options, present_options); - - present_options = selected_drone->getHdgEstimators(); - chooseOptions(odom_hdg_estimtor_options, present_options); + present_options = selected_drone->getOdomEstimators(); + chooseOptions(odom_estimators_options, present_options); } // Set menu actions @@ -194,33 +182,12 @@ boost::shared_ptr ImServer::getMenu() { } menu->addMenu(set_tracker); - for (const auto& option : odom_source_options) { - QAction* action = new QAction(option.c_str(), set_odom_source); + for (const auto& option : odom_estimators_options) { + QAction* action = new QAction(option.c_str(), set_odom_estimator); connect(action, &QAction::triggered, this, [this, option]() { setOdomSources(option); }); - set_odom_source->addAction(action); - } - menu->addMenu(set_odom_source); - - for (const auto& option : odom_lat_estimtor_options) { - QAction* action = new QAction(option.c_str(), set_lat_estimator); - connect(action, &QAction::triggered, this, [this, option]() { setLatEstimators(option); }); - set_lat_estimator->addAction(action); - } - menu->addMenu(set_lat_estimator); - - for (const auto& option : odom_alt_estimtor_options) { - QAction* action = new QAction(option.c_str(), set_alt_estimator); - connect(action, &QAction::triggered, this, [this, option]() { setAltEstimators(option); }); - set_alt_estimator->addAction(action); - } - menu->addMenu(set_alt_estimator); - - for (const auto& option : odom_hdg_estimtor_options) { - QAction* action = new QAction(option.c_str(), set_hdg_estimator); - connect(action, &QAction::triggered, this, [this, option]() { setHdgEstimators(option); }); - set_hdg_estimator->addAction(action); + set_odom_estimator->addAction(action); } - menu->addMenu(set_hdg_estimator); + menu->addMenu(set_odom_estimator); return menu; } diff --git a/src/uav_status/status_display.cpp b/src/uav_status/status_display.cpp index 83ce989..df5b34e 100644 --- a/src/uav_status/status_display.cpp +++ b/src/uav_status/status_display.cpp @@ -228,7 +228,7 @@ void StatusDisplay::drawTopLine() { } else { avoiding_text = "COL AVOID DISABLED"; } - tmp.sprintf("ToF: %3d:%02d %s %s %s %s UAVs:%d", secs_flown / 60, secs_flown % 60, uav_name.c_str(), uav_type.c_str(), nato_name.c_str(), + tmp.sprintf("ToF: %3d:%02d %s %s %s UAVs:%d", secs_flown / 60, secs_flown % 60, uav_name.c_str(), uav_type.c_str(), avoiding_text.c_str(), num_other_uavs); painter.drawStaticText(0, 0, QStaticText(tmp)); @@ -386,13 +386,9 @@ void StatusDisplay::drawOdometry() { painter.drawStaticText(15, 80, QStaticText(value)); // Estimators info - QString hori = QString("Hori: %1").arg(curr_estimator_hori.c_str()); - QString vert = QString("Vert: %1").arg(curr_estimator_vert.c_str()); - QString head = QString("Head: %1").arg(curr_estimator_hdg.c_str()); + QString estimator = QString("%1").arg(curr_estimator.c_str()); painter.drawStaticText(100, 20, QStaticText(odom_frame.c_str())); - painter.drawStaticText(100, 40, QStaticText(hori)); - painter.drawStaticText(100, 60, QStaticText(vert)); - painter.drawStaticText(100, 80, QStaticText(head)); + painter.drawStaticText(100, 40, QStaticText(estimator)); if (!null_tracker) { const double cerr_x = std::fabs(state_x - cmd_x); @@ -879,7 +875,6 @@ void StatusDisplay::uavStatusCb(const mrs_msgs::UavStatusConstPtr& msg) { void StatusDisplay::processTopLine(const mrs_msgs::UavStatusConstPtr& msg) { std::string new_uav_name = msg->uav_name; std::string new_uav_type = msg->uav_type; - std::string new_nato_name = msg->nato_name; bool new_collision_avoidance_enabled = msg->collision_avoidance_enabled; bool new_avoiding_collision = msg->avoiding_collision; bool new_automatic_start_can_takeoff = msg->automatic_start_can_takeoff; @@ -888,7 +883,6 @@ void StatusDisplay::processTopLine(const mrs_msgs::UavStatusConstPtr& msg) { top_line_update_required |= compareAndUpdate(new_uav_name, uav_name); top_line_update_required |= compareAndUpdate(new_uav_type, uav_type); - top_line_update_required |= compareAndUpdate(new_nato_name, nato_name); top_line_update_required |= compareAndUpdate(new_collision_avoidance_enabled, collision_avoidance_enabled); top_line_update_required |= compareAndUpdate(new_avoiding_collision, avoiding_collision); top_line_update_required |= compareAndUpdate(new_automatic_start_can_takeoff, automatic_start_can_takeoff); @@ -937,15 +931,11 @@ void StatusDisplay::processOdometry(const mrs_msgs::UavStatusConstPtr& msg) { double new_cmd_z = msg->cmd_z; double new_cmd_hdg = msg->cmd_hdg; std::string new_odom_frame = msg->odom_frame; - std::string new_curr_estimator_hori; - std::string new_curr_estimator_vert; - std::string new_curr_estimator_hdg; + std::string new_curr_estimator; new_odom_frame = new_odom_frame.substr(new_odom_frame.find("/") + 1); - msg->odom_estimators_hori.empty() ? new_curr_estimator_hori = "NONE" : new_curr_estimator_hori = msg->odom_estimators_hori[0]; - msg->odom_estimators_vert.empty() ? new_curr_estimator_vert = "NONE" : new_curr_estimator_vert = msg->odom_estimators_vert[0]; - msg->odom_estimators_hdg.empty() ? new_curr_estimator_hdg = "NONE" : new_curr_estimator_hdg = msg->odom_estimators_hdg[0]; + msg->odom_estimators.empty() ? new_curr_estimator = "NONE" : new_curr_estimator = msg->odom_estimators[0]; odom_update_required |= compareAndUpdate(new_avg_odom_rate, avg_odom_rate); odom_update_required |= compareAndUpdate(new_color, color); @@ -958,9 +948,7 @@ void StatusDisplay::processOdometry(const mrs_msgs::UavStatusConstPtr& msg) { odom_update_required |= compareAndUpdate(new_cmd_z, cmd_z); odom_update_required |= compareAndUpdate(new_cmd_hdg, cmd_hdg); odom_update_required |= compareAndUpdate(new_odom_frame, odom_frame); - odom_update_required |= compareAndUpdate(new_curr_estimator_hori, curr_estimator_hori); - odom_update_required |= compareAndUpdate(new_curr_estimator_vert, curr_estimator_vert); - odom_update_required |= compareAndUpdate(new_curr_estimator_hdg, curr_estimator_hdg); + odom_update_required |= compareAndUpdate(new_curr_estimator, curr_estimator); } void StatusDisplay::processGeneralInfo(const mrs_msgs::UavStatusConstPtr& msg) { @@ -1051,9 +1039,7 @@ void StatusDisplay::nameUpdate() { // Odometry odom_frame = "!NO DATA!"; - curr_estimator_hori = "!NO DATA!"; - curr_estimator_vert = "!NO DATA!"; - curr_estimator_hdg = "!NO DATA!"; + curr_estimator = "!NO DATA!"; avg_odom_rate = 0.0; odom_update_required = true;