public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage message, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time)
        {
            if (was_full)
            {
                was_full = false;
            }

            var i = new Item
            {
                helper             = helper,
                message            = message,
                nonconst_need_copy = nonconst_need_copy,
                receipt_time       = receipt_time
            };

            lock ( queue )
            {
                if (FullNoLock())
                {
                    queue.Dequeue();
                    was_full = true;
                }
                queue.Enqueue(i);
            }
        }
Beispiel #2
0
 internal MessageAndSerializerFunc(RosMessage msg, TopicManager.SerializeFunc serfunc, bool ser, bool nc)
 {
     this.msg     = msg;
     this.serfunc = serfunc;
     serialize    = ser;
     nocopy       = nc;
 }
Beispiel #3
0
        private async Task HandleConnection()
        {
            // establish connection
            using (var client = new TcpClient())
            {
                await client.ConnectAsync(host, port).ConfigureAwait(false);

                client.NoDelay = true;

                try
                {
                    this.connection = new Connection(client);

                    // write/read header handshake
                    await WriteHeader().ConfigureAwait(false);

                    var headerFields = await connection.ReadHeader(cancel).ConfigureAwait(false);

                    SetHeader(new Header(headerFields));

                    // connection established
                    this.connected = true;

                    while (!cancel.IsCancellationRequested)
                    {
                        // read message length
                        int length = await connection.ReadInt32(cancel).ConfigureAwait(false);

                        if (length > Connection.MESSAGE_SIZE_LIMIT)
                        {
                            var message = $"Message received in TransportPublisherLink exceeds length limit of {Connection.MESSAGE_SIZE_LIMIT}. Dropping connection";
                            throw new RosException(message);
                        }

                        // read message
                        var messageBuffer = await connection.ReadBlock(length, cancel).ConfigureAwait(false);

                        // deserialize message
                        RosMessage msg = RosMessage.Generate(Parent.DataType);
                        msg.Serialized        = messageBuffer;
                        msg.connection_header = this.Header.Values;
                        HandleMessage(msg);

                        // reset retry delay after first successfully processed message
                        retryDelay = BASE_RETRY_DELAY;
                    }

                    client.Close();
                }
                finally
                {
                    this.connected  = false;
                    this.connection = null;
                }
            }
        }
        public virtual MReq deserialize <MReq, MRes>(ServiceCallbackHelperParams <MReq, MRes> parms) where MReq : RosMessage where MRes : RosMessage
        {
            RosMessage msg = RosMessage.Generate(type);

            msg.connection_header = new Dictionary <string, string>(parms.connection_header);
            MReq t = (MReq)msg;

            t.Deserialize(parms.response.Serialized);
            return(t);
        }
Beispiel #5
0
 public void SendEvent(RosMessage msg)
 {
     if (Event != null)
     {
         Event(msg);
     }
     else
     {
         logger.LogError($"{nameof(Event)} is null");
     }
 }
 public void SendEvent(RosMessage msg)
 {
     if (Event != null)
     {
         Event(msg);
     }
     else
     {
         ROS.Error()($"[{ThisNode.Name}] {nameof( Event )} is null");
     }
 }
        public bool call(MReq req, ref MRes resp)
        {
            RosMessage iresp = resp;
            bool       r     = call(req, ref iresp);

            if (iresp != null)
            {
                resp = (MRes)iresp;
            }
            return(r);
        }
Beispiel #8
0
        public void publish(Publication p, RosMessage msg, SerializeFunc serfunc = null)
        {
            if (msg == null)
            {
                return;
            }
            if (serfunc == null)
            {
                serfunc = msg.Serialize;
            }
            if (p.connection_header == null)
            {
                p.connection_header = new Header {
                    Values = new Dictionary <string, string>()
                };
                p.connection_header.Values["type"]               = p.DataType;
                p.connection_header.Values["md5sum"]             = p.Md5sum;
                p.connection_header.Values["message_definition"] = p.MessageDefinition;
                p.connection_header.Values["callerid"]           = ThisNode.Name;
                p.connection_header.Values["latching"]           = Convert.ToString(p.Latch);
            }

            if (!ROS.ok || shuttingDown)
            {
                return;
            }

            if (p.HasSubscribers || p.Latch)
            {
                bool nocopy    = false;
                bool serialize = false;
                if (msg != null && msg.MessageType != "undefined/unknown")
                {
                    p.getPublishTypes(ref serialize, ref nocopy, msg.MessageType);
                }
                else
                {
                    serialize = true;
                }

                p.publish(new MessageAndSerializerFunc(msg, serfunc, serialize, nocopy));

                if (serialize)
                {
                    PollManager.Instance.poll_set.ContinueThreads();
                }
            }
            else
            {
                p.incrementSequence();
            }
        }
        public bool call(RosMessage req, ref RosMessage resp)
        {
            if (resp == null)
            {
                //instantiate null response IN CASE this call succeeds
                resp = RosMessage.Generate(req.MessageType.Replace("Request", "Response"));
            }

            CallInfo info = new CallInfo {
                req = req, resp = resp, success = false, finished = false
            };

            bool immediate = false;

            lock ( call_queue_mutex )
            {
                if (connection.dropped)
                {
                    return(false);
                }

                if (call_queue.Count == 0 && header_written && header_read)
                {
                    immediate = true;
                }

                call_queue.Enqueue(info);
            }

            if (immediate)
            {
                processNextCall();
            }

            while (!info.finished)
            {
                ROS.Debug()($"[{ThisNode.Name}] info.finished_condition.WaitOne();");
                info.finished_condition.WaitOne();
            }

            if (info.success)
            {
                // response is only sent on success => don't try to deserialize on failure.
                resp.Deserialize(resp.Serialized);
            }

            if (!string.IsNullOrEmpty(info.exception))
            {
                ROS.Error()($"[{ThisNode.Name}] Service call failed: service [{name}] responded with an error: {info.exception}");
            }
            return(info.success);
        }
Beispiel #10
0
        public void Publish(Publication p, RosMessage msg, SerializeFunc serfunc = null)
        {
            if (msg == null)
            {
                return;
            }

            if (serfunc == null)
            {
                serfunc = msg.Serialize;
            }

            if (p.connectionHeader == null)
            {
                var fields = new Dictionary <string, string>()
                {
                    ["type"]               = p.DataType,
                    ["md5sum"]             = p.Md5Sum,
                    ["message_definition"] = p.MessageDefinition,
                    ["callerid"]           = ThisNode.Name,
                    ["latching"]           = p.Latch ? "1" : "0"
                };
                p.connectionHeader = new Header(fields);
            }

            if (!ROS.OK || shuttingDown)
            {
                return;
            }

            if (p.HasSubscribers || p.Latch)
            {
                bool nocopy    = false;
                bool serialize = false;
                if (msg != null && msg.MessageType != "undefined/unknown")
                {
                    p.GetPublishTypes(ref serialize, ref nocopy, msg.MessageType);
                }
                else
                {
                    serialize = true;
                }

                p.Publish(new MessageAndSerializerFunc(msg, serfunc, serialize, nocopy));
            }
            else
            {
                p.IncrementSequence();
            }
        }
Beispiel #11
0
        private async Task ProcessCall(CallInfo call)
        {
            RosMessage request = call.Request;

            // serialize and send request
            request.Serialized = request.Serialize();

            await connection.WriteBlock(BitConverter.GetBytes(request.Serialized.Length), 0, 4, cancel).ConfigureAwait(false);

            await connection.WriteBlock(request.Serialized, 0, request.Serialized.Length, cancel).ConfigureAwait(false);

            // read response header
            var receiveBuffer = await connection.ReadBlock(5, cancel).ConfigureAwait(false);

            bool success        = receiveBuffer[0] != 0;
            int  responseLength = BitConverter.ToInt32(receiveBuffer, 1);

            if (responseLength < 0 || responseLength > Connection.MESSAGE_SIZE_LIMIT)
            {
                var errorMessage = $"Message length exceeds limit of {Connection.MESSAGE_SIZE_LIMIT}. Dropping connection.";
                ROS.Error()(errorMessage);
                throw new ConnectionError(errorMessage);
            }

            if (responseLength > 0)
            {
                logger.LogDebug($"Reading message with length of {responseLength}.");
                receiveBuffer = await connection.ReadBlock(responseLength, cancel).ConfigureAwait(false);
            }
            else
            {
                receiveBuffer = new byte[0];
            }

            if (success)
            {
                call.Response.Serialized = receiveBuffer;
                call.Tcs.TrySetResult(true);
            }
            else
            {
                if (receiveBuffer.Length > 0)
                {
                    // call failed with reason
                    call.Tcs.TrySetException(new Exception(Encoding.UTF8.GetString(receiveBuffer)));
                }

                call.Tcs.TrySetResult(false);
            }
        }
Beispiel #12
0
        public static RosService Generate(string rosServiceType)
        {
            var service = ServiceTypeRegistry.Default.CreateService(rosServiceType);

            if (service == null)
            {
                throw new ArgumentException($"Could not find a RosService class for {rosServiceType}.", nameof(rosServiceType));
            }

            service.RequestMessage  = RosMessage.Generate(service.ServiceType + "__Request");
            service.ResponseMessage = RosMessage.Generate(service.ServiceType + "__Response");

            return(service);
        }
Beispiel #13
0
        public async Task <(bool, RosMessage)> Call(RosMessage request)
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            RosMessage response = RosMessage.Generate(request.MessageType.Replace("Request", "Response"));

            if (response == null)
            {
                throw new Exception("Response message generation failed.");
            }

            var queue = callQueue;

            try
            {
                var call = new CallInfo {
                    Request = request, Response = response
                };
                await queue.OnNext(call, cancel).ConfigureAwait(false);

                bool success = await call.AsyncResult.ConfigureAwait(false);

                if (success)
                {
                    // response is only sent on success
                    response.Deserialize(response.Serialized);
                }

                return(success, response);
            }
            catch (Exception e)
            {
                string message = $"Service call failed: service [{name}] responded with an error: {e.Message}";
                ROS.Error()(message);
                return(false, null);
            }
            finally
            {
                if (!persistent)
                {
                    queue.OnCompleted();
                }
            }
        }
Beispiel #14
0
        public async Task <Subscriber> SubscribeAsync(string topic, string messageType, int queueSize, CallbackInterface cb, bool allowConcurrentCallbacks = false)
        {
            if (callbackQueue == null)
            {
                callbackQueue = ROS.GlobalCallbackQueue;
            }

            var message = RosMessage.Generate(messageType);
            var ops     = new SubscribeOptions(topic, message.MessageType, message.MD5Sum(), queueSize, new SubscriptionCallbackHelper <RosMessage>(message.MessageType, cb.SendEvent))
            {
                callback_queue             = callbackQueue,
                allow_concurrent_callbacks = allowConcurrentCallbacks
            };

            ops.callback_queue.AddCallback(cb);
            return(await SubscribeAsync(ops));
        }
Beispiel #15
0
 public virtual async Task ProcessResponse(RosMessage msg, bool success)
 {
     byte[] buf;
     if (success)
     {
         msg.Serialized = msg.Serialize();
         buf            = new byte[msg.Serialized.Length + 1 + 4];
         buf[0]         = (byte)(success ? 0x01 : 0x00);
         msg.Serialized.CopyTo(buf, 5);
         Array.Copy(BitConverter.GetBytes(msg.Serialized.Length), 0, buf, 1, 4);
     }
     else
     {
         buf    = new byte[1 + 4];
         buf[0] = (byte)(success ? 0x01 : 0x00);
         Array.Copy(BitConverter.GetBytes(0), 0, buf, 1, 4);
     }
     await connection.WriteBlock(buf, 0, buf.Length, cancel);
 }
 public virtual void processResponse(RosMessage msg, bool success)
 {
     byte[] buf;
     if (success)
     {
         msg.Serialized = msg.Serialize();
         buf            = new byte[msg.Serialized.Length + 1 + 4];
         buf[0]         = (byte)(success ? 0x01 : 0x00);
         msg.Serialized.CopyTo(buf, 5);
         Array.Copy(BitConverter.GetBytes(msg.Serialized.Length), 0, buf, 1, 4);
     }
     else
     {
         buf    = new byte[1 + 4];
         buf[0] = (byte)(success ? 0x01 : 0x00);
         Array.Copy(BitConverter.GetBytes(0), 0, buf, 1, 4);
     }
     connection.write(buf, buf.Length, onResponseWritten);
 }
Beispiel #17
0
        private bool onMessage(Connection conn, byte[] buffer, int size, bool success)
        {
            if (!success || conn == null || conn != connection)
            {
                return(false);
            }

            if (success)
            {
                RosMessage msg = RosMessage.Generate(parent.msgtype);
                msg.Serialized        = buffer;
                msg.connection_header = getHeader().Values;
                handleMessage(msg, true, false);
            }
            if (success || !connection.transport.getRequiresHeader())
            {
                connection.read(4, onMessageLength);
            }
            return(true);
        }
Beispiel #18
0
 protected void InitSubtypes(RosMessage request, RosMessage response)
 {
     RequestMessage  = request;
     ResponseMessage = response;
 }
Beispiel #19
0
 protected RosMessage GeneralInvoke(RosServiceDelegate invocation, RosMessage m)
 {
     return(invocation.Invoke(m));
 }
 public abstract void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool was_full, TimeData receipt_time);
Beispiel #21
0
 public override void AddToCallbackQueue(ISubscriptionCallbackHelper helper, RosMessage msg, bool nonconst_need_copy, ref bool wasFull, TimeData receiptTime)
 {
     throw new NotImplementedException();
 }
Beispiel #22
0
 public virtual void call(RosMessage msg)
 {
     Callback.SendEvent(msg);
 }
Beispiel #23
0
 public override bool Equals(RosMessage msg)
 {
     return(Equals(msg as FeedbackActionMessage <TFeedback>));
 }
Beispiel #24
0
 public override bool Equals(RosMessage msg)
 {
     return(Equals(msg as GoalActionMessage <TGoal>));
 }
Beispiel #25
0
        internal long HandleMessage(
            RosMessage msg,
            bool ser,
            bool nocopy,
            IDictionary <string, string> connectionHeader,
            PublisherLink link
            )
        {
            RosMessage t            = null;
            long       drops        = 0;
            TimeData   receipt_time = ROS.GetTime().data;

            if (msg.Serialized != null) // will be null if self-subscribed
            {
                msg.Deserialize(msg.Serialized);
            }

            lock (gate)
            {
                foreach (CallbackInfo info in callbacks)
                {
                    string ti = info.Helper.type;
                    if (nocopy || ser)
                    {
                        t = msg;
                        t.connection_header = msg.connection_header;
                        t.Serialized        = null;
                        bool wasFull            = false;
                        bool nonconst_need_copy = callbacks.Count > 1;
                        info.SubscriptionQueue.AddToCallbackQueue(info.Helper, t, nonconst_need_copy, ref wasFull, receipt_time);
                        if (wasFull)
                        {
                            ++drops;
                        }
                        else
                        {
                            info.CallbackQueue.AddCallback(info.SubscriptionQueue, info.SubscriptionQueue);
                        }
                    }
                }
            }

            if (t != null && link.Latched)
            {
                LatchInfo li = new LatchInfo
                {
                    Message          = t,
                    Link             = link,
                    ConnectionHeader = connectionHeader,
                    ReceiptTime      = receipt_time
                };
                if (latchedMessages.ContainsKey(link))
                {
                    latchedMessages[link] = li;
                }
                else
                {
                    latchedMessages.Add(link, li);
                }
            }

            return(drops);
        }
 public override bool Equals(RosMessage msg)
 {
     return(Equals(msg as WrappedFeedbackMessage <T>));
 }