void fcuStatusCallback(const asctec_hl_comm::mav_statusConstPtr& status) {
// Details of this message can found in the source of the implementation at:
// http://ros.org/rosdoclite/groovy/api/asctec_hl_comm/html/HL__interface_8h.html
// http://ros.org/rosdoclite/groovy/api/asctec_hl_comm/html/HL__interface_8h_source.html
// http://ros.org/rosdoclite/groovy/api/asctec_hl_interface/html/hl__interface_8cpp_source.html
std::ostringstream label;
int min, sec;
// **** get GTK thread lock
gdk_threads_enter();
// **** update gauge1: battery voltage [mV]
// UPDATE: After testing with the Pelican+fcu, it turns out the unit is [V]
if (IS_GTK_GAUGE (data->gauge1))
gtk_gauge_set_value(GTK_GAUGE (data->gauge1), status->battery_voltage); // * 0.001);
// possible flight modes are: "Acc", "Height", "GPS" and "unknown"
gtk_label_set_text(GTK_LABEL (data->flightMode_label), status->flight_mode_ll.c_str());
// flight time [s]
// format time to 00:00 (min:sec)
min = int(status->flight_time) / 60;
sec = int(status->flight_time) % 60;
if (min < 10)
label << "0" << min << ":";
else
label << min << ":";
if (sec < 10)
label << "0" << sec;
else
label << sec;
gtk_label_set_text(GTK_LABEL (data->upTime_label), label.str().c_str());
// remember to empty ostringstream
label.str("");
// CPU load in % of the time of one SDK loop
label << status->cpu_load << "%";
gtk_label_set_text(GTK_LABEL (data->cpuLoad_label), label.str().c_str());
label.str("");
// Number of GPS Satellites (int32)
label << int(status->gps_num_satellites);
gtk_label_set_text(GTK_LABEL (data->numSats_label), label.str().c_str());
// possible modes are: "off", "stopping", "starting" and "running"
if (status->motor_status.compare("off") == 0) {
gtk_container_remove(GTK_CONTAINER(data->box_MotorStatus), GTK_WIDGET (data->status_ok_icon_motor));
gtk_box_pack_end(GTK_BOX (data->box_MotorStatus), data->status_fail_icon_motor, TRUE, TRUE, 0);
}
else {
gtk_container_remove(GTK_CONTAINER(data->box_MotorStatus), GTK_WIDGET (data->status_fail_icon_motor));
gtk_box_pack_end(GTK_BOX (data->box_MotorStatus), data->status_ok_icon_motor, TRUE, TRUE, 0);
}
// **** release GTK thread lock
gdk_threads_leave();
}
void llStatusCallback (const asctec_msgs::LLStatusConstPtr & dat)
{
// **** get GTK thread lock
gdk_threads_enter ();
llStatus_ = (*dat);
char buf[FILENAME_MAX];
// **** update altimeter
if (IS_GTK_GAUGE (data->gauge1))
gtk_gauge_set_value (GTK_GAUGE (data->gauge1), (double) (llStatus_.battery_voltage_1 / 1000.));
/*if(llStatus_.flying==0)
{
gtk_container_remove(GTK_CONTAINER(data->box_Flying),GTK_WIDGET (data->status_ok_icon_flying));
gtk_box_pack_end (GTK_BOX (data->box_Flying),data->status_fail_icon_flying, TRUE, TRUE, 0);
}
else
{
gtk_container_remove(GTK_CONTAINER(data->box_Flying),GTK_WIDGET (data->status_fail_icon_flying));
gtk_box_pack_end (GTK_BOX (data->box_Flying),data->status_ok_icon_flying, TRUE, TRUE, 0);
}
if(llStatus_.motors_on==0)
{
gtk_container_remove(GTK_CONTAINER(data->box_MotorStatus),GTK_WIDGET (data->status_ok_icon_motor));
gtk_box_pack_end (GTK_BOX (data->box_MotorStatus),data->status_fail_icon_motor, TRUE, TRUE, 0);
}
else
{
gtk_container_remove(GTK_CONTAINER(data->box_MotorStatus),GTK_WIDGET (data->status_fail_icon_motor));
gtk_box_pack_end (GTK_BOX (data->box_MotorStatus),data->status_ok_icon_motor, TRUE, TRUE, 0);
} */
sprintf (buf, "%d", llStatus_.flightMode);
gtk_label_set_text (GTK_LABEL (data->flightMode_label), buf);
sprintf (buf, "%d", llStatus_.up_time);
gtk_label_set_text (GTK_LABEL (data->upTime_label), buf);
sprintf (buf, "%d", llStatus_.cpu_load);
gtk_label_set_text (GTK_LABEL (data->cpuLoad_label), buf);
// **** release GTK thread lock
gdk_threads_leave ();
}
/**
* @fn int main (int argc, char **argv)
* @brief Main program & Gtk thread.
*
* Create window and all widgets, then set there parameters to be the <br>
* ROS params.
*/
int main (int argc, char **argv)
{
GtkBuilder *builder;
GdkColor black = { 0, 0, 0, 0 };
GError *error = NULL;
char glade_gui_file[FILENAME_MAX];
int start_zoom = 15;
char *mapcachedir;
OsmGpsMapPoint ccny_coord = { 40.818551, -73.948674 };
struct arg param;
param.argc = argc;
param.argv = argv;
pthread_t rosThread;
// **** init threads
g_thread_init (NULL);
gdk_threads_init ();
gdk_threads_enter ();
// **** init gtk
gtk_init (&argc, &argv);
// **** allocate data structure
data = g_slice_new (AppData);
// **** set the glade gui file & set icon directory
std::string package_path = ros::package::getPath (ROS_PACKAGE_NAME);
sprintf (glade_gui_file, "%s/gui/%s", package_path.c_str (), "gui.glade");
sprintf (data->icon_directory, "%s/gui/icon", package_path.c_str ());
std::string rosbag_path = ros::package::getPath("rosbag");
sprintf (data->rosbag_rec_path, "%s/bin/record", rosbag_path.c_str ());
data->current_page = 0;
data->telemetry_opt_popup_state = false;
data->gps_opt_popup_state = false;
data->fullscreen = false;
load_icon ();
// **** Create new GtkBuilder object
builder = gtk_builder_new ();
// **** Load UI from file
if (!gtk_builder_add_from_file (builder, glade_gui_file, &error))
{
g_warning ("%s", error->message);
g_free (error);
exit (-1);
}
// **** Get main window pointer from UI
data->window = GTK_WIDGET (gtk_builder_get_object (builder, "window1"));
gtk_window_set_title (GTK_WINDOW (data->window), "CityFlyer Ground Station");
gtk_window_set_position (GTK_WINDOW (data->window), GTK_WIN_POS_CENTER);
gtk_window_set_default_size (GTK_WINDOW (data->window), 1024, 576);
// **** create ROS thread
pthread_create (&rosThread, NULL, startROS, ¶m);
// **** wait ros finish read params
while (!data->ros_param_read)
{
ROS_DEBUG ("Waiting ROS params");
}
// **** Get GtkNotebook objsect
data->notebook = GTK_WIDGET (gtk_builder_get_object (builder, "notebook1"));
// #####################################################################
// #####################################################################
// **** Tab 1: Telemetry
// **** create altimeter widgets
data->alt = gtk_altimeter_new ();
g_object_set (GTK_ALTIMETER (data->alt),
"grayscale-color", data->grayscale_color,
"unit-is-feet", data->altimeter_unit_is_feet,
"unit-step-value", data->altimeter_step_value, "radial-color", data->radial_color, NULL);
// **** create compass widgets
data->comp = gtk_compass_new ();
g_object_set (GTK_COMPASS (data->comp),
"grayscale-color", data->grayscale_color, "radial-color", data->radial_color, NULL);
data->comp2 = gtk_compass_new ();
g_object_set (GTK_COMPASS (data->comp2),
"grayscale-color", data->grayscale_color, "radial-color", data->radial_color, NULL);
data->gauge1 = gtk_gauge_new ();
g_object_set (GTK_GAUGE (data->gauge1), "name", data->gauge1_name_f, NULL);
g_object_set (GTK_GAUGE (data->gauge1),
"grayscale-color", data->grayscale_color,
"radial-color", data->radial_color,
"start-value", data->gauge1_start_value,
"end-value", data->gauge1_end_value,
"initial-step", data->gauge1_initial_step,
"sub-step", (gdouble) data->gauge1_sub_step,
"drawing-step", data->gauge1_drawing_step,
"color-strip-order", data->gauge1_color_strip_order,
"green-strip-start", data->gauge1_green_strip_start,
"yellow-strip-start", data->gauge1_yellow_strip_start,
"red-strip-start", data->gauge1_red_strip_start, NULL);
// **** create artificial horizon widgets
data->arh = gtk_artificial_horizon_new ();
g_object_set (GTK_ARTIFICIAL_HORIZON (data->arh),
"grayscale-color", data->grayscale_color, "radial-color", data->radial_color, NULL);
// **** create variometer widgets
data->vario = gtk_variometer_new ();
g_object_set (GTK_VARIOMETER (data->vario),
"grayscale-color", data->grayscale_color,
"unit-is-feet", data->variometer_unit_is_feet,
"unit-step-value", data->variometer_step_value, "radial-color", data->radial_color, NULL);
data->widget_table = GTK_WIDGET (gtk_builder_get_object (builder, "table_Widgets"));
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->alt, 0, 1, 0, 1);
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->arh, 1, 2, 0, 1);
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->comp, 2, 3, 0, 1);
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->vario, 0, 1, 1, 2);
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->comp2, 1, 2, 1, 2);
gtk_table_attach_defaults (GTK_TABLE (data->widget_table), data->gauge1, 2, 3, 1, 2);
gtk_widget_modify_bg (data->alt, GTK_STATE_NORMAL, &black);
gtk_widget_modify_bg (data->comp, GTK_STATE_NORMAL, &black);
gtk_widget_modify_bg (data->comp2, GTK_STATE_NORMAL, &black);
gtk_widget_modify_bg (data->arh, GTK_STATE_NORMAL, &black);
gtk_widget_modify_bg (data->gauge1, GTK_STATE_NORMAL, &black);
gtk_widget_modify_bg (data->vario, GTK_STATE_NORMAL, &black);
data->telemetry_option_popup = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_TelemetryOption"));
data->btn_open_telemetry_option_popup =
GTK_WIDGET (gtk_builder_get_object (builder, "button_OpenTelemetryOptionPopup"));
data->btn_close_telemetry_option_popup =
GTK_WIDGET (gtk_builder_get_object (builder, "button_CloseTelemetryOptionPopup"));
gtk_button_set_image (GTK_BUTTON (data->btn_open_telemetry_option_popup),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->leftarrow_icon_64, 24, 50, GDK_INTERP_HYPER)));
gtk_button_set_image (GTK_BUTTON (data->btn_close_telemetry_option_popup),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->rightarrow_icon_64, 24, 50, GDK_INTERP_HYPER)));
// #####################################################################
// #####################################################################
// **** Tab 2: Gps
// Some GpsdViewer initialisation
data->draw_path = false;
data->map_provider = OSM_GPS_MAP_SOURCE_OPENSTREETMAP;
data->map_zoom_max = 18;
data->map_current_zoom = start_zoom;
data->repo_uri = osm_gps_map_source_get_repo_uri (data->map_provider);
data->friendly_name = osm_gps_map_source_get_friendly_name (data->map_provider);
data->uav_track = osm_gps_map_track_new ();
mapcachedir = osm_gps_map_get_default_cache_directory ();
data->cachedir = g_build_filename (mapcachedir, data->friendly_name, NULL);
g_free (mapcachedir);
// Create the OsmGpsMap object
data->map = (OsmGpsMap *) g_object_new (OSM_TYPE_GPS_MAP,
"map-source", data->map_provider,
"tile-cache", data->cachedir, "proxy-uri", g_getenv ("http_proxy"), NULL);
//Set the starting coordinates and zoom level for the map
osm_gps_map_set_zoom (data->map, start_zoom);
osm_gps_map_set_center (data->map, ccny_coord.rlat, ccny_coord.rlon);
data->osd = gpsd_viewer_osd_new ();
g_object_set (GPSD_VIEWER_OSD (data->osd),
"show-scale", true,
"show-coordinates", true,
"show-dpad", true,
"show-zoom", true, "show-gps-in-dpad", true, "show-gps-in-zoom", false, "dpad-radius", 30, NULL);
osm_gps_map_layer_add (OSM_GPS_MAP (data->map), OSM_GPS_MAP_LAYER (data->osd));
data->map_box = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_map_box"));
data->map_container = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_map_container"));
gtk_box_pack_start (GTK_BOX (data->map_box), GTK_WIDGET (data->map), TRUE, TRUE, 0);
data->gpsd_option_popup = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_GpsdOptionPopup"));
data->btn_open_gpsd_option_popup = GTK_WIDGET (gtk_builder_get_object (builder, "button_OpenGpsdOptionPopup"));
data->btn_close_gpsd_option_popup = GTK_WIDGET (gtk_builder_get_object (builder, "button_CloseGpsdOptionPopup"));
gtk_button_set_image (GTK_BUTTON (data->btn_open_gpsd_option_popup),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->leftarrow_icon_64, 24, 50, GDK_INTERP_HYPER)));
gtk_button_set_image (GTK_BUTTON (data->btn_close_gpsd_option_popup),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->rightarrow_icon_64, 24, 50, GDK_INTERP_HYPER)));
// #####################################################################
// #####################################################################
// **** Tab 3: Rec
data->recording = 0;
data->rosbag_record_cmd = "rosbag record";
data->topicsList = GTK_LIST_STORE (gtk_builder_get_object (builder, "liststore_TopicList"));
data->cmd_line_entry = GTK_WIDGET (gtk_builder_get_object (builder, "entry_CommandLine"));
data->prefix_entry = GTK_WIDGET (gtk_builder_get_object (builder, "entry_Prefix"));
data->info_textview = GTK_WIDGET (gtk_builder_get_object (builder, "textview_BagInfo"));
data->update_btn = GTK_WIDGET (gtk_builder_get_object (builder, "button_UpdateTopicList"));
data->box_MotorStatus = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_MotorStatus"));
data->box_Flying = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_Flying"));
data->box_Gps = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_Gps"));
data->flightMode_label = GTK_WIDGET (gtk_builder_get_object (builder, "label_FlightModeValue"));
data->upTime_label = GTK_WIDGET (gtk_builder_get_object (builder, "label_UpTimeValue"));
data->cpuLoad_label = GTK_WIDGET (gtk_builder_get_object (builder, "label_CpuLoadValue"));
data->box_RecordStatus = GTK_WIDGET (gtk_builder_get_object (builder, "hbox_RecordStatus"));
data->record_stop_btn = GTK_WIDGET (gtk_builder_get_object (builder, "button_RecordStop"));
gtk_box_pack_end (GTK_BOX (data->box_MotorStatus), data->status_ok_icon_motor, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_MotorStatus), data->status_fail_icon_motor, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_Flying), data->status_ok_icon_flying, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_Flying), data->status_fail_icon_flying, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_Gps), data->status_ok_icon_gps, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_Gps), data->status_fail_icon_gps, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_RecordStatus), data->record_icon, TRUE, TRUE, 0);
gtk_box_pack_end (GTK_BOX (data->box_RecordStatus), data->record_g_icon, TRUE, TRUE, 0);
gtk_button_set_image (GTK_BUTTON (data->update_btn),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->refresh_icon_64, 24, 24, GDK_INTERP_HYPER)));
gtk_button_set_image (GTK_BUTTON (data->record_stop_btn),
gtk_image_new_from_pixbuf (gdk_pixbuf_scale_simple
(data->record_icon_64, 40, 40, GDK_INTERP_HYPER)));
// Connect signals
gtk_builder_connect_signals (builder, data);
// Destroy builder, since we don't need it anymore
g_object_unref (G_OBJECT (builder));
// Show window. All other widgets are automatically shown by GtkBuilder
gtk_widget_show_all (data->window);
gtk_widget_hide(data->record_icon);
gtk_widget_hide(data->status_ok_icon_motor);
gtk_widget_hide(data->status_ok_icon_flying);
gtk_widget_hide(data->status_ok_icon_gps);
gtk_widget_hide_all(data->telemetry_option_popup);
gtk_widget_hide_all(data->gpsd_option_popup);
// **** allow ROS spinning
data->widget_created = true;
// **** udpate all widgets
g_timeout_add (data->telemetry_refresh_rate, widgets_update, NULL);
gtk_main ();
gdk_threads_leave ();
return 0;
}
