Skip to content

Commit

Permalink
updating for compability with new status msg
Browse files Browse the repository at this point in the history
  • Loading branch information
@DanHert
DanHert committed Nov 24, 2023
1 parent 95a4e6b commit e975f35
Show file tree
Hide file tree
Showing 6 changed files with 69 additions and 133 deletions.
2 changes: 1 addition & 1 deletion include/control/control.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -89,4 +89,4 @@ class ControlTool : public rviz::SelectionTool{

} // namespace mrs_rviz_plugins

#endif
#endif
11 changes: 4 additions & 7 deletions include/control/drone_entity.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,10 +41,7 @@ class DroneEntity {
std::vector<std::string> getGains();
std::vector<std::string> getControllers();
std::vector<std::string> getTrackers();
std::vector<std::string> getOdomSources();
std::vector<std::string> getLatEstimators();
std::vector<std::string> getAltEstimators();
std::vector<std::string> getHdgEstimators();
std::vector<std::string> getOdomEstimators();
bool getNullTracker();

void setServiceNumCalls(const int value);
Expand Down Expand Up @@ -126,10 +123,10 @@ class DroneEntity {
std::vector<std::string> controllers{};
std::vector<std::string> trackers{};
std::vector<std::string>
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<std::string> odom_alt_sources{};
std::vector<std::string> odom_hdg_sources{};
/* std::vector<std::string> odom_alt_sources{}; */
/* std::vector<std::string> odom_hdg_sources{}; */


// | ----------------------- Attributes ----------------------- |
Expand Down
56 changes: 27 additions & 29 deletions include/uav_status/status_display.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,8 @@ private Q_SLOTS:
std::pair<int, int> getSpawnCoordinates(rviz::Property* property);
std::pair<int, int> 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) {
Expand Down Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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<std::string, bool> taken_uavs;

// | --------------------- Default values --------------------- |
Expand Down
28 changes: 8 additions & 20 deletions src/control/drone_entity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand All @@ -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;

Expand Down Expand Up @@ -451,20 +451,8 @@ std::vector<std::string> DroneEntity::getTrackers() {
return trackers;
}

std::vector<std::string> DroneEntity::getOdomSources() {
return odom_lat_sources;
}

std::vector<std::string> DroneEntity::getLatEstimators() {
return odom_lat_sources;
}

std::vector<std::string> DroneEntity::getAltEstimators() {
return odom_alt_sources;
}

std::vector<std::string> DroneEntity::getHdgEstimators() {
return odom_hdg_sources;
std::vector<std::string> DroneEntity::getOdomEstimators() {
return odom_estimators;
}

bool DroneEntity::getNullTracker() {
Expand Down
77 changes: 22 additions & 55 deletions src/control/im_server.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,17 +96,17 @@ boost::shared_ptr<QMenu> 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);
Expand All @@ -129,14 +129,11 @@ boost::shared_ptr<QMenu> ImServer::getMenu() {
// Filter the options that are not present in every selected drone
DroneEntity* drone = selected_drones[0];

std::vector<std::string> constraint_options = drone->getConstraints();
std::vector<std::string> gain_options = drone->getGains();
std::vector<std::string> controller_options = drone->getControllers();
std::vector<std::string> tracker_options = drone->getTrackers();
std::vector<std::string> odom_source_options = drone->getOdomSources();
std::vector<std::string> odom_lat_estimtor_options = drone->getLatEstimators();
std::vector<std::string> odom_alt_estimtor_options = drone->getAltEstimators();
std::vector<std::string> odom_hdg_estimtor_options = drone->getHdgEstimators();
std::vector<std::string> constraint_options = drone->getConstraints();
std::vector<std::string> gain_options = drone->getGains();
std::vector<std::string> controller_options = drone->getControllers();
std::vector<std::string> tracker_options = drone->getTrackers();
std::vector<std::string> odom_estimators_options = drone->getOdomEstimators();
std::vector<std::string> present_options;

for (auto& selected_drone : selected_drones) {
Expand All @@ -152,17 +149,8 @@ boost::shared_ptr<QMenu> 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
Expand Down Expand Up @@ -194,33 +182,12 @@ boost::shared_ptr<QMenu> 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;
}
Expand Down
28 changes: 7 additions & 21 deletions src/uav_status/status_display.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));

Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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);
Expand All @@ -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) {
Expand Down Expand Up @@ -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;

Expand Down

0 comments on commit e975f35

Please sign in to comment.