public emTransform transformFrame(string source, string target, out gm.Vector3 vec, out gm.Quaternion quat)
{
emTransform trans = new emTransform();
try
{
transformer.lookupTransform(target, source, new Time(new TimeData()), out trans);
}
catch (Exception e)
{
ROS.Error(e.ToString());
trans = null;
}
if (trans != null)
{
vec = trans.translation != null?trans.translation.ToMsg() : new emVector3().ToMsg();
quat = trans.rotation != null?trans.rotation.ToMsg() : new emQuaternion().ToMsg();
}
else
{
vec = null;
quat = null;
}
return(trans);
}
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;
}
}
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(),
response = new MRes(),
connection_header = link.connection.header.Values
};
parms.request.Deserialize(buffer);
try
{
bool ok = isp.helper.call(parms);
link.processResponse(parms.response, ok);
}
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);
}
public bool handleHeader(Header header)
{
if (!header.Values.Contains("md5sum") || !header.Values.Contains("service") || !header.Values.Contains("callerid"))
{
string bbq = "Bogus tcpros header. did not have required elements: md5sum, service, callerid";
ROS.Error(bbq);
connection.sendHeaderError(ref bbq);
return(false);
}
string md5sum = (string)header.Values["md5sum"];
string service = (string)header.Values["service"];
string client_callerid = (string)header.Values["client_callerid"];
if (header.Values.Contains("persistent") && ((string)header.Values["persistent"] == "1" || (string)header.Values["persistent"] == "true"))
{
persistent = true;
}
ROS.Debug("Service client [{0}] wants service [{1}] with md5sum [{2}]", client_callerid, service, md5sum);
IServicePublication isp = ServiceManager.Instance.lookupServicePublication(service);
if (isp == null)
{
string bbq = string.Format("received a tcpros connection for a nonexistent service [{0}]", service);
ROS.Error(bbq);
connection.sendHeaderError(ref bbq);
return(false);
}
if (isp.md5sum != md5sum && md5sum != "*" && isp.md5sum != "*")
{
string bbq = "client wants service " + service + " to have md5sum " + md5sum + " but it has " + isp.md5sum + ". Dropping connection";
ROS.Error(bbq);
connection.sendHeaderError(ref bbq);
return(false);
}
if (isp.isDropped)
{
string bbq = "received a tcpros connection for a nonexistent service [" + service + "]";
ROS.Error(bbq);
connection.sendHeaderError(ref bbq);
return(false);
}
parent = isp;
IDictionary m = new Hashtable();
m["request_type"] = isp.req_datatype;
m["response_type"] = isp.res_datatype;
m["type"] = isp.datatype;
m["md5sum"] = isp.md5sum;
m["callerid"] = this_node.Name;
connection.writeHeader(m, onHeaderWritten);
isp.addServiceClientLink(this);
return(true);
}
public void lookupTransform(string target_frame, string source_frame, Time time, out emTransform transform, ref string error_string)
{
transform = new emTransform();
string mapped_tgt = resolve(tf_prefix, target_frame);
string mapped_src = resolve(tf_prefix, source_frame);
if (mapped_tgt == mapped_src)
{
transform.translation = new emVector3();
transform.rotation = new emQuaternion();
transform.child_frame_id = mapped_src;
transform.frame_id = mapped_tgt;
transform.stamp = ROS.GetTime(DateTime.Now);
return;
}
lock (framemutex)
{
uint target_id = getFrameID(mapped_tgt);
uint source_id = getFrameID(mapped_src);
TransformAccum accum = new TransformAccum();
TF_STATUS retval = walkToTopParent(accum, TimeCache.toLong(time.data), target_id, source_id, ref error_string);
if (error_string != null && retval != TF_STATUS.NO_ERROR)
{
switch (retval)
{
case TF_STATUS.CONNECTIVITY_ERROR:
ROS.Error("NO CONNECTIONSZSZ: " + error_string);
break;
case TF_STATUS.EXTRAPOLATION_ERROR:
ROS.Error("EXTRAPOLATION: " + error_string);
break;
case TF_STATUS.LOOKUP_ERROR:
ROS.Error("LOOKUP: " + error_string);
break;
}
}
transform.translation = accum.result_vec;
transform.rotation = accum.result_quat;
transform.child_frame_id = mapped_src;
transform.frame_id = mapped_tgt;
transform.stamp = new Time {
data = new TimeData {
sec = (uint)(accum.time >> 32), nsec = (uint)(accum.time & 0xFFFFFFFF)
}
};
}
}
public void lookupTransform(String t, String s, Time time, out emTransform transform)
{
try
{
lookupTransform(t.data, s.data, time, out transform);
}
catch (Exception e)
{
transform = null;
ROS.Error(e);
throw e;
}
}
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();
}
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("Service call failed: service [{0}] responded with an error: {1}", name, info.exception);
}
return(info.success);
}
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);
}
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);
}
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);
}
