public bool call(IRosMessage req, ref IRosMessage resp)
{
CallInfo info = new CallInfo {
req = req, resp = resp, success = false, finished = false, call_finished = false, caller_thread_id = ROS.getPID()
};
bool immediate = false;
lock (call_queue_mutex)
{
if (connection.dropped)
{
info.call_finished = true;
return(false);
}
if (call_queue.Count == 0 && header_written && header_read)
{
immediate = true;
}
call_queue.Enqueue(info);
}
if (immediate)
{
processNextCall();
}
while (!info.finished)
{
info.finished_condition.WaitOne();
}
lock (info.finished_mutex)
{
info.call_finished = true;
}
resp = resp.Deserialize(resp.Serialized);
if (!string.IsNullOrEmpty(info.exception))
{
ROS.Error("Service call failed: service [{0}] responded with an error: {1}", name, info.exception);
}
return(info.success);
}
public SimTime()
{
new Thread(() =>
{
while (!ROS.isStarted() && !ROS.shutting_down)
{
Thread.Sleep(100);
}
Thread.Sleep(1000);
if (!ROS.shutting_down)
{
nh = new NodeHandle();
simTimeSubscriber = nh.subscribe <Messages.rosgraph_msgs.Clock>("/clock", 1, SimTimeCallback);
}
ROS.waitForShutdown();
simTimeSubscriber.shutdown();
}).Start();
}
private void SimTimeCallback(Messages.rosgraph_msgs.Clock time)
{
if (!checkedSimTime)
{
if (Param.get("/use_sim_time", ref simTime))
{
checkedSimTime = true;
if (simTime)
{
ROS.Warn("Switching to sim time");
}
}
}
if (simTime && SimTimeEvent != null)
{
SimTimeEvent(TimeSpan.FromMilliseconds(time.clock.data.sec * 1000.0 + (time.clock.data.nsec / 100000000.0)));
}
}
public bool call(IRosMessage req, ref IRosMessage resp)
{
if (resp == null)
{
//instantiate null response IN CASE this call succeeds
resp = IRosMessage.generate((MsgTypes)Enum.Parse(typeof(MsgTypes), req.msgtype.ToString().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)
{
info.finished_condition.WaitOne();
}
resp.Deserialize(resp.Serialized);
if (!string.IsNullOrEmpty(info.exception))
{
ROS.Error("Service call failed: service [{0}] responded with an error: {1}", name, info.exception);
}
return(info.success);
}
public static bool exists(string service_name, bool print_failure_reason)
{
string mapped_name = names.resolve(service_name);
string host = "";
int port = 0;
if (ServiceManager.Instance.lookUpService(mapped_name, ref host, ref port))
{
TcpTransport transport = new TcpTransport();
IDictionary m = new Hashtable
{
{ "probe", "1" },
{ "md5sum", "*" },
{ "callerid", this_node.Name },
{ "service", mapped_name }
};
byte[] headerbuf = null;
int size = 0;
Header h = new Header();
h.Write(m, ref headerbuf, ref size);
if (transport.connect(host, port))
{
byte[] sizebuf = BitConverter.GetBytes(size);
transport.write(sizebuf, 0, sizebuf.Length);
transport.write(headerbuf, 0, size);
return(true);
}
if (print_failure_reason)
{
ROS.Info("waitForService: Service[{0}] could not connect to host [{1}:{2}], waiting...", mapped_name, host, port);
}
}
else if (print_failure_reason)
{
ROS.Info("waitForService: Service[{0}] has not been advertised, waiting...", mapped_name);
}
return(false);
}
public bool EnqueueMessage(IRosMessage msg)
{
lock (subscriber_links_mutex)
{
if (Dropped)
{
return(false);
}
}
uint seq = incrementSequence();
if (HasHeader)
{
object h = msg.GetType().GetField("header").GetValue(msg);
m.Header header;
if (h == null)
{
header = new m.Header();
}
else
{
header = (m.Header)h;
}
header.seq = seq;
header.stamp = ROS.GetTime();
header.frame_id = new m.String();
msg.GetType().GetField("header").SetValue(msg, header);
}
msg.connection_header = connection_header.Values;
lock (subscriber_links_mutex)
foreach (SubscriberLink sub_link in subscriber_links)
{
sub_link.enqueueMessage(msg, true, false);
}
if (Latch)
{
last_message = msg;
}
return(true);
}
public void addCallbacks(SubscriberCallbacks callbacks)
{
lock (callbacks_mutex)
{
this.callbacks.Add(callbacks);
if (callbacks.connect != null && callbacks.Callback != null)
{
lock (subscriber_links_mutex)
{
foreach (SubscriberLink i in subscriber_links)
{
CallbackInterface cb = new PeerConnDisconnCallback(callbacks.connect, i);
callbacks.Callback.addCallback(cb, ROS.getPID());
}
}
}
}
}
private bool onResponseOkAndLength(Connection conn, byte[] buf, int size, bool success)
{
if (conn != connection || size != 5)
{
throw new Exception("response or length NOT OK!");
}
if (!success)
{
return(false);
}
byte ok = buf[0];
int len = BitConverter.ToInt32(buf, 1);
if (len > 1000000000)
{
ROS.Error("GIGABYTE IS TOO BIIIIG");
connection.drop(Connection.DropReason.Destructing);
return(false);
}
lock (call_queue_mutex)
{
if (ok != 0)
{
current_call.success = true;
}
else
{
current_call.success = false;
}
}
if (len > 0)
{
connection.read(len, onResponse);
}
else
{
byte[] f = new byte[0];
onResponse(conn, f, 0, true);
}
return(true);
}
private void construct(string ns, bool validate_name)
{
if (!ROS.initialized)
{
ROS.FREAKOUT();
}
collection = new NodeHandleBackingCollection();
UnresolvedNamespace = ns;
Namespace = validate_name ? resolveName(ns) : resolveName(ns, true, true);
ok = true;
lock (nh_refcount_mutex)
{
if (nh_refcount == 0 && !ROS.isStarted())
{
node_started_by_nh = true;
ROS.start();
}
++nh_refcount;
}
}
public virtual void onRequestLength(Connection conn, ref byte[] buffer, uint size, bool success)
{
if (!success)
{
return;
}
if (conn != connection || size != 4)
{
throw new Exception("Invalid request length read");
}
uint len = BitConverter.ToUInt32(buffer, 0);
if (len > 1000000000)
{
ROS.Error("A message over a gigabyte was predicted... stop... being... bad.");
connection.drop(Connection.DropReason.Destructing);
return;
}
connection.read(len, onRequest);
}
private bool onHeaderReceived(Connection conn, Header header)
{
string md5sum;
if (header.Values.Contains("md5sum"))
{
md5sum = (string)header.Values["md5sum"];
}
else
{
ROS.Error("TCPROS header from service server did not have required element: md5sum");
return(false);
}
//TODO check md5sum
bool empty = false;
lock (call_queue_mutex)
{
empty = call_queue.Count == 0;
if (empty)
{
header_read = true;
}
}
IsValid = true;
if (!empty)
{
processNextCall();
header_read = true;
}
return(true);
}
internal override CallResult Call()
{
if (link.connection.dropped)
{
return(CallResult.Invalid);
}
ServiceCallbackHelperParams <MReq, MRes> parms = new ServiceCallbackHelperParams <MReq, MRes>
{
request = new MReq().Deserialize(buffer) as MReq,
response = new MRes(),
connection_header = link.connection.header.Values
};
try
{
bool ok = isp.helper.call(parms);
if (ok)
{
link.processResponse(parms.response, true);
}
else
{
IRosMessage res = new MRes();
link.processResponse(res, false);
}
}
catch (Exception e)
{
string woops = "Exception thrown while processing service call: " + e;
ROS.Error(woops);
link.processResponse(woops, false);
return(CallResult.Invalid);
}
return(CallResult.Success);
}
private TF_STATUS getLatestCommonTime(uint target_id, uint source_id, ref ulong time, ref string error_str)
{
if (target_id == source_id)
{
time = TimeCache.toLong(ROS.GetTime(DateTime.Now).data);
return(TF_STATUS.NO_ERROR);
}
List <TimeAndFrameID> lct = new List <TimeAndFrameID>();
uint frame = source_id;
TimeAndFrameID temp;
uint depth = 0;
ulong common_time = ulong.MaxValue;
while (frame != 0)
{
TimeCache cache;
if (!frames.ContainsKey(frame))
{
break;
}
cache = frames[frame];
TimeAndFrameID latest = cache.getLatestTimeAndParent();
if (latest.frame_id == 0)
{
break;
}
if (latest.time != 0)
{
common_time = Math.Min(latest.time, common_time);
}
lct.Add(latest);
frame = latest.frame_id;
if (frame == target_id)
{
time = common_time;
if (time == ulong.MaxValue)
{
time = 0;
}
return(TF_STATUS.NO_ERROR);
}
++depth;
if (depth > MAX_GRAPH_DEPTH)
{
if (error_str != null)
{
error_str = "The tf tree is invalid because it contains a loop.";
}
return(TF_STATUS.LOOKUP_ERROR);
}
}
frame = target_id;
depth = 0;
common_time = ulong.MaxValue;
uint common_parent = 0;
while (true)
{
TimeCache cache;
if (!frames.ContainsKey(frame))
{
break;
}
cache = frames[frame];
TimeAndFrameID latest = cache.getLatestTimeAndParent();
if (latest.frame_id == 0)
{
break;
}
if (latest.time != 0)
{
common_time = Math.Min(latest.time, common_time);
}
foreach (TimeAndFrameID tf in lct)
{
if (tf.frame_id == latest.frame_id)
{
common_parent = tf.frame_id;
break;
}
}
frame = latest.frame_id;
if (frame == source_id)
{
time = common_time;
if (time == uint.MaxValue)
{
time = 0;
}
return(TF_STATUS.NO_ERROR);
}
++depth;
if (depth > MAX_GRAPH_DEPTH)
{
if (error_str != null)
{
error_str = "The tf tree is invalid because it contains a loop.";
}
return(TF_STATUS.LOOKUP_ERROR);
}
}
if (common_parent == 0)
{
error_str = "" + frameids_reverse[source_id] + " DOES NOT CONNECT TO " + frameids_reverse[target_id];
return(TF_STATUS.CONNECTIVITY_ERROR);
}
for (int i = 0; i < lct.Count; i++)
{
if (lct[i].time != 0)
{
common_time = Math.Min(common_time, lct[i].time);
}
if (lct[i].frame_id == common_parent)
{
break;
}
}
if (common_time == uint.MaxValue)
{
common_time = 0;
}
time = common_time;
return(TF_STATUS.NO_ERROR);
}
public UInt64 Get()
{
return(ROS.getPID());
}
internal ulong handleMessage(IRosMessage msg, bool ser, bool nocopy, IDictionary connection_header,
PublisherLink link)
{
lock (callbacks_mutex)
{
ulong drops = 0;
cached_deserializers.Clear();
TimeData receipt_time = ROS.GetTime().data;
foreach (ICallbackInfo info in callbacks)
{
MsgTypes ti = info.helper.type;
if (nocopy || ser)
{
IMessageDeserializer deserializer = null;
if (cached_deserializers.ContainsKey(ti))
{
deserializer = cached_deserializers[ti];
}
else
{
deserializer = MakeDeserializer(ti, info.helper, msg, connection_header);
cached_deserializers.Add(ti, deserializer);
}
bool was_full = false;
bool nonconst_need_copy = callbacks.Count > 1;
//info.helper.callback().func(msg);
//info.helper.callback().pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time);
info.subscription_queue.pushitgood(info.helper, deserializer, nonconst_need_copy, ref was_full, receipt_time);
//push(info.helper, deserializer, nonconst_need_copy, ref was_full,receipt_time);
if (was_full)
{
++drops;
}
else
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
if (link.Latched)
{
LatchInfo li = new LatchInfo
{
message = msg,
link = link,
connection_header = connection_header,
receipt_time = receipt_time
};
if (latched_messages.ContainsKey(link))
{
latched_messages[link] = li;
}
else
{
latched_messages.Add(link, li);
}
}
cached_deserializers.Clear();
return(drops);
}
}
public IMessageEvent(IRosMessage msg)
: this(msg, msg.connection_header, ROS.GetTime().data, true, DefaultCreator)
{
}
public MessageEvent (T msg)
{
message = msg;
connectionHeader = new Dictionary<string, string> ();
receiptTime = ROS.GetTime ();
}
public virtual void addCallback(CallbackInterface callback)
{
addCallback(callback, ROS.getPID());
}
internal bool addCallback <M>(SubscriptionCallbackHelper <M> helper, string md5sum, CallbackQueueInterface queue,
uint queue_size, bool allow_concurrent_callbacks, string topiclol) where M : IRosMessage, new()
{
lock (md5sum_mutex)
{
if (this.md5sum == "*" && md5sum != "*")
{
this.md5sum = md5sum;
}
}
if (md5sum != "*" && md5sum != this.md5sum)
{
return(false);
}
lock (callbacks_mutex)
{
CallbackInfo <M> info = new CallbackInfo <M> {
helper = helper, callback = queue, subscription_queue = new Callback <M>(helper.callback().func, topiclol, queue_size, allow_concurrent_callbacks)
};
//if (!helper.isConst())
//{
++nonconst_callbacks;
//}
callbacks.Add(info);
if (latched_messages.Count > 0)
{
MsgTypes ti = info.helper.type;
lock (publisher_links_mutex)
{
foreach (PublisherLink link in publisher_links)
{
if (link.Latched)
{
if (latched_messages.ContainsKey(link))
{
LatchInfo latch_info = latched_messages[link];
bool was_full = false;
bool nonconst_need_copy = false; //callbacks.Count > 1;
info.subscription_queue.pushitgood(info.helper, latched_messages[link].message, nonconst_need_copy, ref was_full, ROS.GetTime().data);
if (!was_full)
{
info.callback.addCallback(info.subscription_queue, info.Get());
}
}
}
}
}
}
}
return(true);
}
public override void processRequest(ref byte[] buf, int num_bytes, IServiceClientLink link)
{
CallbackInterface cb = new ServiceCallback(this, helper, buf, num_bytes, link, has_tracked_object, tracked_object);
callback.addCallback(cb, ROS.getPID());
}
