diff options
| -rw-r--r-- | src/uca-kiro-camera.c | 549 |
1 files changed, 549 insertions, 0 deletions
diff --git a/src/uca-kiro-camera.c b/src/uca-kiro-camera.c new file mode 100644 index 0000000..3483f9d --- /dev/null +++ b/src/uca-kiro-camera.c @@ -0,0 +1,549 @@ +/* Copyright (C) 2011, 2012 Matthias Vogelgesang <matthias.vogelgesang@kit.edu> + (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 <gmodule.h> +#include <gio/gio.h> +#include <string.h> +#include <math.h> +#include <kiro/kiro-messenger.h> +#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://<IP>:<PORT>') property was supplied during camera creation + @UCA_KIRO_CAMERA_ERROR_ADDRESS_WRONG_FORMAT: KIRO address has the wrong format. Expected: 'kiro://<IP>:<PORT>' + @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, }; + +struct _UcaKiroCameraPrivate { + guint8 *dummy_data; + guint current_frame; + gchar *kiro_address_string; + gchar *kiro_address; + gchar *kiro_port; + gchar *remote_name; + GParamSpec **kiro_dynamic_attributes; + + gboolean thread_running; + gboolean kiro_connected; + gboolean construction_error; + + GThread *grab_thread; + KiroMessenger *messenger; + gulong peer_rank; + + 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 KiroContinueFlag +receive_handler (KiroMessageStatus *status, gpointer user_data) +{ + UcaKiroCamera *cam = (UcaKiroCamera *)user_data; + UcaKiroCameraPrivate *priv = UCA_KIRO_CAMERA_GET_PRIVATE (cam); + + KiroMessage *msg = status->message; + + if (msg->msg == KIROCS_READY) { + g_debug ("Interface Setup Done."); + priv->kiro_connected = TRUE; + return KIRO_CALLBACK_CONTINUE; + } + + g_message ("Message Type '%u' is unhandled.", msg->msg); + return KIRO_CALLBACK_CONTINUE; +} + + +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->kiro_connected = FALSE; + + kiro_messenger_add_receive_callback (priv->messenger, receive_handler, kiro_camera); + 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; + kiro_messenger_remove_receive_callback (priv->messenger); + return; + } + + //Wait until the remote side has given us the "READY" signal + while (!priv->kiro_connected) {}; + + if (priv->construction_error) { + //something went wrong. Tear down the connection. + 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; + } + + GError *error = NULL; + + GVariant *tmp = variant_from_scalar (value); + gsize data_size = g_variant_get_size (tmp); + + PropUpdate *test = g_malloc0 (sizeof (PropUpdate) + data_size); + test->id = property_id_from_name (pspec->name); + test->type[0] = gtype_to_gvariant_class (pspec->value_type); + test->size = data_size; + g_variant_store (tmp, test->val); + g_variant_unref (tmp); + + KiroMessage message; + message.peer_rank = priv->peer_rank; + message.msg = KIROCS_UPDATE; + message.payload = test; + message.size = sizeof (PropUpdate) + data_size; + + kiro_messenger_send_blocking (priv->messenger, &message, &error); + if (error) { + g_free (test); + g_error ("Oh shit! (%s)", error->message); + } + + g_free (test); + } +} + + +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: + //try_handle_read_tango_property (object, property_id, value, pspec); + 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 Server and TANGO 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 remot camera", + "Name of the camera plugin that is loaded on the KIRO remote site", + "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->kiro_address = g_strdup ("NONE"); + self->priv->kiro_port = g_strdup ("NONE"); + self->priv->remote_name = g_strdup ("NONE"); + self->priv->construction_error = FALSE; + self->priv->kiro_dynamic_attributes = 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; +} + |