void imuCalcDataCallback (const asctec_msgs::IMUCalcDataConstPtr & dat)
{
// **** get GTK thread lock
gdk_threads_enter ();
imuCalcData_ = (*dat);
ROS_DEBUG("imuCalcData yaw %f, pitch %f, roll %f\n",(double)imuCalcData_.angle_yaw /1000.,(double)imuCalcData_.angle_nick/1000.,(double)imuCalcData_.angle_roll/1000.);
if (IS_GTK_VARIOMETER (data->vario))
gtk_variometer_set_value (GTK_VARIOMETER (data->vario), (double) (imuCalcData_.dheight /1000.)*3600);
if (IS_GTK_COMPASS (data->comp))
gtk_compass_set_angle (GTK_COMPASS (data->comp), (double) (imuCalcData_.mag_heading / 1000.));
// **** release GTK thread lock
gdk_threads_leave ();
}
// legacy asctec_autopilot-related callback function implementations
void imuCallback(const sensor_msgs::ImuConstPtr & imu) {
// **** get GTK thread lock
gdk_threads_enter();
imuData_ = (*imu);
double roll, pitch, yaw;
tf::Quaternion orientation;
orientation.setX(imuData_.orientation.x);
orientation.setY(imuData_.orientation.y);
orientation.setZ(imuData_.orientation.z);
orientation.setW(imuData_.orientation.w);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
ROS_DEBUG("Yaw %f, Pitch %f, Roll %f, RollBis %f", RAD2DEG(yaw),
RAD2DEG(pitch), RAD2DEG(roll), (double)(((int)(RAD2DEG(roll)*1000)+360000)%360000)/1000);
//~ if (IS_GTK_ALTIMETER (data->alt))
//~ gtk_altimeter_set_alti (GTK_ALTIMETER (data->alt), (double) (imuCalcData_.height / 1000.));
//~
//~ if (IS_GTK_VARIOMETER (data->vario))
//~ gtk_variometer_set_value (GTK_VARIOMETER (data->vario), (double) (imuCalcData_.dheight / 1000.)*3600);
//~
//~ if (IS_GTK_COMPASS (data->comp))
//~ gtk_compass_set_angle (GTK_COMPASS (data->comp), (double) (imuCalcData_.mag_heading / 1000.));
if (IS_GTK_COMPASS (data->comp2))
gtk_compass_set_angle(GTK_COMPASS (data->comp2),
(double) (((int) (RAD2DEG(yaw) * 1000) + 360000) % 360000)
/ 1000);
if (IS_GTK_ARTIFICIAL_HORIZON (data->arh))
gtk_artificial_horizon_set_value(GTK_ARTIFICIAL_HORIZON (data->arh),
(double) (((int) (RAD2DEG(roll) * 1000) + 360000) % 360000)
/ 1000, (double) -RAD2DEG(pitch));
// **** 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;
}