/* Copyright (C) 2011, 2012 Matthias Vogelgesang (Karlsruhe Institute of Technology) This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public License as published by the Free Software Foundation; either version 2.1 of the License, or (at your option) any later version. This library is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License for more details. You should have received a copy of the GNU Lesser General Public License along with this library; if not, write to the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110, USA */ #include #include #include #include #include #include "uca-kiro-camera.h" #define UCA_KIRO_CAMERA_GET_PRIVATE(obj) (G_TYPE_INSTANCE_GET_PRIVATE ((obj), UCA_TYPE_KIRO_CAMERA, UcaKiroCameraPrivate)) static void uca_kiro_initable_iface_init (GInitableIface *iface); GError *initable_iface_error = NULL; G_DEFINE_TYPE_WITH_CODE (UcaKiroCamera, uca_kiro_camera, UCA_TYPE_CAMERA, G_IMPLEMENT_INTERFACE (G_TYPE_INITABLE, uca_kiro_initable_iface_init)) /** * UcaCameraError: @UCA_KIRO_CAMERA_ERROR_MISSING_ADDRESS: No KIRO address ('kiro://:') property was supplied during camera creation @UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT: KIRO address has the wrong format. Expected: 'kiro://:' @UCA_KIRO_CAMERA_ERROR_KIRO_CONNECTION_FAILED: Failed to establish a KIRO connection to the given TANGO server */ GQuark uca_kiro_camera_error_quark() { return g_quark_from_static_string ("uca-kiro-camera-error-quark"); } enum { PROP_KIRO_ADDRESS = N_BASE_PROPERTIES, PROP_KIRO_REMOTE_NAME, N_PROPERTIES }; static const gint kiro_overrideables[] = { PROP_NAME, 0, }; static GParamSpec *kiro_properties[N_PROPERTIES] = { NULL, }; //+1 to make index and property ID identical static guint64 kiro_scalar_prop_buffers[N_PROPERTIES] = { 0, }; typedef struct { GParamSpec *pspec; guint32 remote_id; guint32 local_id; guint64 buffer; } KiroDynamicScalarProperty; struct _UcaKiroCameraPrivate { guint8 *dummy_data; guint current_frame; gchar *kiro_address_string; gchar *kiro_address; gchar *kiro_port; gchar *remote_name; GList *property_install_list; KiroDynamicScalarProperty *kiro_dynamic_scalar_properties; gboolean thread_running; gboolean kiro_connected; gboolean construction_error; GThread *grab_thread; KiroMessenger *messenger; gulong peer_rank; KiroRequest *rec_request; guint roi_height; guint roi_width; guint bytes_per_pixel; }; static gpointer kiro_grab_func(gpointer data) { /* UcaKiroCamera *kiro_camera = UCA_KIRO_CAMERA (data); */ /* g_return_val_if_fail (UCA_IS_KIRO_CAMERA (kiro_camera), NULL); */ /* UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (kiro_camera); */ /* UcaCamera *camera = UCA_CAMERA (kiro_camera); */ /* gdouble fps; */ /* g_object_get (G_OBJECT (data), "frames-per-second", &fps, NULL); */ /* const gulong sleep_time = (gulong) G_USEC_PER_SEC / fps; */ /* while (priv->thread_running) { */ /* camera->grab_func (NULL, camera->user_data); */ /* g_usleep (sleep_time); */ /* } */ return NULL; } static void uca_kiro_camera_start_recording(UcaCamera *camera, GError **error) { /* gboolean transfer_async = FALSE; */ /* UcaKiroCameraPrivate *priv; */ /* g_return_if_fail(UCA_IS_KIRO_CAMERA (camera)); */ /* priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */ /* g_object_get (G_OBJECT(camera), */ /* "transfer-asynchronously", &transfer_async, */ /* NULL); */ /* //'Cache' ROI settings from TANGO World */ /* g_object_get (G_OBJECT(camera), */ /* "roi-width", &priv->roi_width, */ /* NULL); */ /* g_object_get (G_OBJECT(camera), */ /* "roi-height", &priv->roi_height, */ /* NULL); */ /* size_t bits = 0; */ /* g_object_get (G_OBJECT(camera), */ /* "sensor-bitdepth", &bits, */ /* NULL); */ /* priv->bytes_per_pixel = 1; */ /* if (bits > 8) priv->bytes_per_pixel++; */ /* if (bits > 16) priv->bytes_per_pixel++; */ /* if (bits > 24) priv->bytes_per_pixel++; */ /* Tango::DevState state; */ /* g_object_get (G_OBJECT(camera), */ /* "State", &state, */ /* NULL); */ /* try { */ /* if (Tango::DevState::STANDBY == state) */ /* priv->tango_device->command_inout ("StartRecording"); */ /* } */ /* catch (Tango::DevFailed &e) { */ /* g_warning ("Failed to execute 'StartRecording' on the remote camera due to a TANGO exception.\n"); */ /* g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_TANGO_EXCEPTION_OCCURED, */ /* "A TANGO exception was raised: '%s'", (const char *)e.errors[0].desc); */ /* return; */ /* } */ /* /* */ /* * In case asynchronous transfer is requested, we start a new thread that */ /* * invokes the grab callback, otherwise nothing will be done here. */ /* *1/ */ /* if (transfer_async) { */ /* GError *tmp_error = NULL; */ /* priv->thread_running = TRUE; */ /* priv->grab_thread = g_thread_create (kiro_grab_func, camera, TRUE, &tmp_error); */ /* if (tmp_error != NULL) { */ /* priv->thread_running = FALSE; */ /* g_propagate_error (error, tmp_error); */ /* try { */ /* priv->tango_device->command_inout ("StopRecording"); */ /* } */ /* catch (Tango::DevFailed &e) { */ /* g_warning ("Failed to execute 'StopRecording' on the remote camera due to a TANGO exception: '%s'\n", (const char *)e.errors[0].desc); */ /* } */ /* } */ /* } */ /* kiro_sb_thaw (priv->receive_buffer); */ } static void uca_kiro_camera_stop_recording(UcaCamera *camera, GError **error) { /* g_return_if_fail(UCA_IS_KIRO_CAMERA (camera)); */ /* UcaKiroCameraPrivate *priv; */ /* priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */ /* Tango::DevState state; */ /* g_object_get (G_OBJECT(camera), */ /* "State", &state, */ /* NULL); */ /* try { */ /* if (Tango::DevState::RUNNING == state) */ /* priv->tango_device->command_inout ("StopRecording"); */ /* } */ /* catch (Tango::DevFailed &e) { */ /* g_warning ("Failed to execute 'StopRecording' on the remote camera due to a TANGO exception.\n"); */ /* g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_TANGO_EXCEPTION_OCCURED, */ /* "A TANGO exception was raised: '%s'", (const char *)e.errors[0].desc); */ /* } */ /* gboolean transfer_async = FALSE; */ /* g_object_get(G_OBJECT (camera), */ /* "transfer-asynchronously", &transfer_async, */ /* NULL); */ /* if (transfer_async) { */ /* priv->thread_running = FALSE; */ /* g_thread_join (priv->grab_thread); */ /* } */ /* kiro_sb_freeze (priv->receive_buffer); */ /* g_free (priv->dummy_data); */ } static void uca_kiro_camera_trigger (UcaCamera *camera, GError **error) { } static gboolean uca_kiro_camera_grab (UcaCamera *camera, gpointer data, GError **error) { /* g_return_val_if_fail (UCA_IS_KIRO_CAMERA (camera), FALSE); */ /* UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (camera); */ /* //This is a hack to make sure we actually wait for a new frame; */ /* gpointer frame = kiro_sb_get_data_blocking (priv->receive_buffer); */ /* kiro_sb_freeze (priv->receive_buffer); */ /* //Element 0 might still be in the process of being written. */ /* //Therefore, we take Element 1, to be sure this one is finished. */ /* if (data) */ /* g_memmove (data, frame, priv->roi_width * priv->roi_height * priv->bytes_per_pixel); */ /* kiro_sb_thaw (priv->receive_buffer); */ return TRUE; } static gboolean kiro_address_decode (const gchar *addr_in, gchar **addr, gchar **port, GError **error) { if (!g_str_has_prefix (addr_in, "kiro://")) { g_set_error_literal (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT, "Address does not use 'kiro://' scheme."); return FALSE; } /* Pitfall: kiro will silently accept hostnames like kiro://localhost:5555 * but not bind to it as it treats it like an interface name (like eth0). * We have to use IP addresses instead of DNS names. */ gchar *host = g_strdup (&addr_in[7]); if (!g_ascii_isdigit (host[0]) && host[0] != '*') g_debug ("Treating address %s as interface device name. Use IP address if supplying a host was intended.", host); gchar **split = g_strsplit (host, ":", 2); if (!g_ascii_isdigit (*split[1])) { g_set_error (error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT, "Address '%s' has wrong format", addr_in); g_strfreev (split); g_free (host); return FALSE; } *addr = g_strdup (split[0]); *port = g_strdup (split[1]); g_strfreev (split); g_free (host); return TRUE; } static void null_callback (gpointer unused) { (void)unused; } void receive_handler (KiroRequest *request, gpointer user_data) { UcaKiroCamera *cam = (UcaKiroCamera *)user_data; UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (cam); KiroMessage *msg = request->message; if (msg->msg == KIROCS_INSTALL) { PropertyRequisition *req = (PropertyRequisition *)msg->payload; g_debug ("Got requisition for a non-base property '%s' of type '%s' with ID %u", req->name, g_type_name (req->value_type), req->id); priv->property_install_list = g_list_append (priv->property_install_list, msg->payload); msg->payload = NULL; goto done; } if (msg->msg == KIROCS_READY) { g_debug ("Interface Setup Done."); priv->kiro_connected = TRUE; goto done; } if (msg->msg == KIROCS_UPDATE) { PropUpdate *update = (PropUpdate *)request->message->payload; const gchar *name = NULL; gpointer buffer = NULL; if (update->scalar == TRUE) { PropUpdateScalar *scalar_update = (PropUpdateScalar *)request->message->payload; if (update->id >= N_BASE_PROPERTIES) { name = priv->kiro_dynamic_scalar_properties[update->id - N_BASE_PROPERTIES].pspec->name; buffer = &priv->kiro_dynamic_scalar_properties[update->id - N_BASE_PROPERTIES].buffer; } else { name = uca_camera_props[update->id]; buffer = &kiro_scalar_prop_buffers[update->id]; } memcpy (buffer, &scalar_update->prop_raw, sizeof (guint64)); } g_debug ("Peer informed us about an update of property '%s' (ID: %u)", name, update->id); goto done; } g_message ("Message Type '%u' is unhandled.", msg->msg); done: if (msg->payload) g_free (msg->payload); g_free (msg); kiro_messenger_receive (priv->messenger, request); } static void uca_kiro_camera_clone_interface(UcaKiroCamera *kiro_camera) { UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (kiro_camera); UcaKiroCameraClass *klass = UCA_KIRO_CAMERA_GET_CLASS (kiro_camera); GObjectClass *gobject_class = G_OBJECT_CLASS (klass); if (priv->messenger) { kiro_messenger_free (priv->messenger); priv->messenger = kiro_messenger_new (); } priv->rec_request = g_malloc0 (sizeof (KiroRequest)); priv->rec_request->id = 0; priv->rec_request->callback = (KiroMessageCallbackFunc) receive_handler; priv->rec_request->user_data = (gpointer) kiro_camera; kiro_messenger_receive (priv->messenger, priv->rec_request); priv->kiro_connected = FALSE; kiro_messenger_connect (priv->messenger, priv->kiro_address, priv->kiro_port, &priv->peer_rank, &initable_iface_error); if (initable_iface_error) { priv->construction_error = TRUE; g_free (priv->rec_request); return; } //Wait until the remote side has given us the "READY" signal while (!priv->kiro_connected) {}; guint count = g_list_length (priv->property_install_list); if (count > 0) { g_debug ("Registering buffers for %u dyanmic properties", count); priv->kiro_dynamic_scalar_properties = g_malloc0 (count * sizeof (KiroDynamicScalarProperty)); GList *curr = g_list_first (priv->property_install_list); guint idx = 0; while (curr) { PropertyRequisition *req = (PropertyRequisition *)curr->data; g_debug ("Registering dynamic property '%s'", req->name); GParamSpec *pspec = g_param_spec_boolean (req->name, "Remote Property", "Remote Property", TRUE, G_PARAM_READWRITE); guint local_id = N_PROPERTIES + idx; g_object_class_install_property (gobject_class, local_id, pspec); priv->kiro_dynamic_scalar_properties[idx].local_id = local_id; priv->kiro_dynamic_scalar_properties[idx].remote_id = req->id; priv->kiro_dynamic_scalar_properties[idx].pspec = pspec; KiroMessage message; message.msg = KIROCS_FETCH; message.size = strlen (req->name) + 1; //Don't forget the NULL-byte message.payload = &(req->name); g_debug ("Sending request to fetch value of property '%s'", req->name); GError *error = NULL; kiro_messenger_send_blocking (priv->messenger, &message, priv->peer_rank, &error); if (error) { g_error ("Oh shit! (%s)", error->message); g_error_free (error); } idx++; curr = g_list_next (curr); } g_list_free (priv->property_install_list); } if (priv->construction_error) { kiro_messenger_stop (priv->messenger); //TODO //Maybe set an error? } } static void uca_kiro_camera_set_property(GObject *object, guint property_id, const GValue *value, GParamSpec *pspec) { g_return_if_fail(UCA_IS_KIRO_CAMERA (object)); UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (object); switch (property_id) { case PROP_KIRO_ADDRESS: if (priv->kiro_address_string) g_free (priv->kiro_address_string); priv->kiro_address_string = g_value_dup_string (value); break; default: g_debug ("Updating %s.", pspec->name); if (!priv->kiro_connected) { g_warning ("Trying to modify a property before a connection to the remote camera was established."); G_OBJECT_WARN_INVALID_PROPERTY_ID(object, property_id, pspec); return; } if (property_id >= N_PROPERTIES) { g_debug ("Non-Base-Property ID %u (Index: %u)", property_id, property_id - N_PROPERTIES); g_value_write_to_raw_data (value, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer)); } else g_value_write_to_raw_data (value, &kiro_scalar_prop_buffers[property_id]); KiroMessage message; message.msg = KIROCS_UPDATE; message.size = sizeof (PropUpdate) + sizeof (guint64); message.payload = g_malloc0 (message.size); gpointer data_pointer = message.payload + sizeof (PropUpdate); PropUpdate *update = (PropUpdate *)message.payload; update->size = 1; update->scalar = TRUE; if (property_id > N_BASE_PROPERTIES) { update->id = priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].remote_id; memcpy (data_pointer, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer), sizeof (guint64)); } else { update->id = property_id; memcpy (data_pointer, &kiro_scalar_prop_buffers[property_id], sizeof (guint64)); } GError *error = NULL; kiro_messenger_send_blocking (priv->messenger, &message, priv->peer_rank, &error); if (error) { g_error ("Oh shit! (%s)", error->message); g_error_free (error); } g_free (message.payload); } } static void uca_kiro_camera_get_property(GObject *object, guint property_id, GValue *value, GParamSpec *pspec) { UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (object); switch (property_id) { case PROP_NAME: g_value_set_string (value, "KIRO camera"); break; case PROP_KIRO_ADDRESS: g_value_set_string (value, priv->kiro_address_string); break; case PROP_KIRO_REMOTE_NAME: g_value_set_string (value, priv->remote_name); break; default: //TODO: //Handle non-scalar types specifically if (property_id <= N_BASE_PROPERTIES) { g_value_set_from_raw_data (value, &kiro_scalar_prop_buffers[property_id]); } else { g_value_set_from_raw_data (value, &(priv->kiro_dynamic_scalar_properties[property_id - N_PROPERTIES].buffer)); } break; } } static void uca_kiro_camera_finalize(GObject *object) { UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE(object); if (priv->thread_running) { priv->thread_running = FALSE; g_thread_join (priv->grab_thread); } if (priv->messenger) { kiro_messenger_free (priv->messenger); priv->messenger = NULL; } priv->kiro_connected = FALSE; if (priv->dummy_data) { g_free (priv->dummy_data); priv->dummy_data = NULL; } g_free (priv->kiro_address_string); g_free (priv->kiro_address); g_free (priv->kiro_port); G_OBJECT_CLASS (uca_kiro_camera_parent_class)->finalize(object); } static gboolean ufo_kiro_camera_initable_init (GInitable *initable, GCancellable *cancellable, GError **error) { g_return_val_if_fail (UCA_IS_KIRO_CAMERA (initable), FALSE); UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (UCA_KIRO_CAMERA (initable)); if(priv->construction_error) { g_propagate_error (error, initable_iface_error); return FALSE; } return TRUE; } static void uca_kiro_initable_iface_init (GInitableIface *iface) { iface->init = ufo_kiro_camera_initable_init; } static void uca_kiro_camera_constructed (GObject *object) { //Initialization for the KIRO Messenger and interface cloning is moved //here and done early! //We want to add dynamic properties and it is too late to do so in the //real initable part. Therefore, we do it here and 'remember' any errors //that occur and check them later in the initable part. UcaKiroCamera *self = UCA_KIRO_CAMERA (object); UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (self); priv->construction_error = FALSE; GValue address = G_VALUE_INIT; g_value_init(&address, G_TYPE_STRING); uca_kiro_camera_get_property (object, PROP_KIRO_ADDRESS, &address, NULL); const gchar *addrstring = g_value_get_string (&address); gint address_not_none = g_strcmp0(addrstring, "NONE"); if (0 == address_not_none) { g_warning ("kiro-address was not set! Can not connect to server...\n"); priv->construction_error = TRUE; g_set_error (&initable_iface_error, UCA_KIRO_CAMERA_ERROR, UCA_KIRO_CAMERA_ERROR_MISSING_ADDRESS, "'kiro-address' property was not set during construction."); } else { if (kiro_address_decode (addrstring, &priv->kiro_address, &priv->kiro_port, &initable_iface_error)) { uca_kiro_camera_clone_interface (self); } else { priv->construction_error = TRUE; } } G_OBJECT_CLASS (uca_kiro_camera_parent_class)->constructed(object); } static void uca_kiro_camera_class_init(UcaKiroCameraClass *klass) { GObjectClass *gobject_class = G_OBJECT_CLASS( klass); gobject_class->set_property = uca_kiro_camera_set_property; gobject_class->get_property = uca_kiro_camera_get_property; gobject_class->finalize = uca_kiro_camera_finalize; gobject_class->constructed = uca_kiro_camera_constructed; UcaCameraClass *camera_class = UCA_CAMERA_CLASS (klass); camera_class->start_recording = uca_kiro_camera_start_recording; camera_class->stop_recording = uca_kiro_camera_stop_recording; camera_class->grab = uca_kiro_camera_grab; camera_class->trigger = uca_kiro_camera_trigger; for (guint i = 0; i < N_BASE_PROPERTIES; i++) g_object_class_override_property (gobject_class, i, uca_camera_props[i]); kiro_properties[PROP_KIRO_ADDRESS] = g_param_spec_string("kiro-address", "KIRO Server Address", "Address of the KIRO Server to grab images from", "NONE", G_PARAM_READWRITE | G_PARAM_CONSTRUCT); kiro_properties[PROP_KIRO_REMOTE_NAME] = g_param_spec_string("remote-name", "Name of the remote camera", "Name of the camera plugin that is loaded on the KIRO remote side", "NONE", G_PARAM_READABLE); for (guint id = N_BASE_PROPERTIES; id < N_PROPERTIES; id++) g_object_class_install_property (gobject_class, id, kiro_properties[id]); g_type_class_add_private (klass, sizeof(UcaKiroCameraPrivate)); } static void uca_kiro_camera_init(UcaKiroCamera *self) { self->priv = UCA_KIRO_CAMERA_GET_PRIVATE(self); self->priv->grab_thread = NULL; self->priv->current_frame = 0; self->priv->kiro_address_string = g_strdup ("NONE"); self->priv->remote_name = g_strdup ("NONE"); self->priv->construction_error = FALSE; self->priv->kiro_dynamic_scalar_properties = NULL; self->priv->messenger = kiro_messenger_new (); self->priv->peer_rank = 0; } G_MODULE_EXPORT GType uca_camera_get_type (void) { return UCA_TYPE_KIRO_CAMERA; }