/// <summary>
/// Kills all the things. Called by checkForShutdown
/// </summary>
private static void _shutdown()
{
lock (shutting_down_mutex)
{
if (shutting_down)
{
return;
}
shutting_down = true;
_ok = false;
EDB.WriteLine("ROS is shutting down.");
GlobalCallbackQueue.Disable();
GlobalCallbackQueue.Clear();
if (started)
{
TopicManager.Instance.shutdown();
ServiceManager.Instance.shutdown();
PollManager.Instance.shutdown();
XmlRpcManager.Instance.shutdown();
if (rosoutappender != null)
{
rosoutappender.shutdown();
}
}
started = false;
_ok = false;
}
}
/// <summary>
/// Creates a publisher with the given advertise options
/// </summary>
/// <typeparam name="M">Type of topic</typeparam>
/// <param name="ops">Advertise options</param>
/// <returns>A publisher with the specified options</returns>
public Publisher <M> advertise <M>(AdvertiseOptions <M> ops) where M : IRosMessage, new()
{
if (started_by_visual_studio)
{
return(null);
}
ops.topic = resolveName(ops.topic);
if (ops.callback_queue == null)
{
ops.callback_queue = Callback;
}
SubscriberCallbacks callbacks = new SubscriberCallbacks(ops.connectCB, ops.disconnectCB, ops.callback_queue);
if (TopicManager.Instance.advertise(ops, callbacks))
{
Publisher <M> pub = new Publisher <M>(ops.topic, ops.md5sum, ops.datatype, this, callbacks);
lock (collection.mutex)
{
collection.publishers.Add(pub);
}
return(pub);
}
EDB.WriteLine("ADVERTISE FAILED!!!!");
return(null);
}
private void onConnectionDropped(Connection conn, Connection.DropReason reason)
{
#if DEBUG
EDB.WriteLine("TransportPublisherLink: onConnectionDropped -- " + reason);
#endif
if (dropping || conn != connection)
{
return;
}
if (reason == Connection.DropReason.TransportDisconnect)
{
needs_retry = true;
next_retry = DateTime.Now.Add(retry_period);
if (retry_timer == null)
{
retry_timer = ROS.timer_manager.StartTimer(onRetryTimer, 100);
}
else
{
retry_timer.Restart();
}
}
else
{
if (reason == Connection.DropReason.HeaderError)
{
EDB.WriteLine("SOMETHING BE WRONG WITH THE HEADER FOR: " +
(parent != null ? parent.name : "unknown"));
}
drop();
}
}
public bool Parse(byte[] buffer, int size, ref string error_msg)
{
int i = 0;
while (i < size)
{
int thispiece = BitConverter.ToInt32(buffer, i);
i += 4;
byte[] line = new byte[thispiece];
Array.Copy(buffer, i, line, 0, thispiece);
string thisheader = Encoding.ASCII.GetString(line);
string[] chunks = thisheader.Split('=');
if (chunks.Length != 2)
{
i += thispiece;
continue;
}
Values[chunks[0].Trim()] = chunks[1].Trim();
i += thispiece;
}
bool res = (i == size);
if (!res)
{
EDB.WriteLine("OH NOES CONNECTION HEADER FAILED TO PARSE!");
}
return(res);
}
/// <summary>
/// Checks if the given topic is valid.
/// </summary>
/// <typeparam name="T">Advertise Options </typeparam>
/// <param name="ops"></param>
/// <returns></returns>
private bool isValid <T>(AdvertiseOptions <T> ops) where T : IRosMessage, new()
{
if (ops.datatype == "*")
{
throw new Exception("Advertising with * as the datatype is not allowed. Topic [" + ops.topic + "]");
}
if (ops.md5sum == "*")
{
throw new Exception("Advertising with * as the md5sum is not allowed. Topic [" + ops.topic + "]");
}
if (ops.md5sum == "")
{
throw new Exception("Advertising on topic [" + ops.topic + "] with an empty md5sum");
}
if (ops.datatype == "")
{
throw new Exception("Advertising on topic [" + ops.topic + "] with an empty datatype");
}
if (ops.message_definition == "")
{
EDB.WriteLine
("Danger, Will Robinson... Advertising on topic [" + ops.topic +
"] with an empty message definition. Some tools (that don't exist in this implementation) may not work correctly");
}
return(true);
}
protected bool precall(string service_md5sum)
{
if (service_md5sum != md5sum)
{
EDB.WriteLine("Call to service [{0} with md5sum [{1} does not match md5sum when the handle was created([{2}])", service, service_md5sum, md5sum);
return(false);
}
if (server_link != null && server_link.connection.dropped)
{
if (persistent)
{
EDB.WriteLine("WARNING: persistent service client's server link has been dropped. trying to reconnect to proceed with this call");
}
server_link = null;
}
if (is_shutdown && persistent)
{
EDB.WriteLine("WARNING: persistent service client is self-resurrecting");
}
is_shutdown = false;
if (persistent && server_link == null || !persistent)
{
server_link = linkmaker();
}
return(true);
}
private void socketUpdate(int events)
{
ScopedTimer.Ping();
lock (close_mutex)
{
if (closed)
{
return;
}
}
if (is_server)
{
TcpTransport transport = accept();
if (transport != null)
{
if (accept_cb == null)
{
throw new Exception("NULL ACCEPT_CB FTL!");
}
accept_cb(transport);
}
}
else
{
if ((events & POLLIN) != 0 && expecting_read) //POLL IN FLAG
{
if (read_cb != null)
{
read_cb(this);
}
}
if ((events & POLLOUT) != 0 && expecting_write)
{
if (write_cb != null)
{
write_cb(this);
}
}
if ((events & POLLERR) != 0 || (events & POLLHUP) != 0 || (events & POLLNVAL) != 0)
{
int error = 0;
try
{
error = (int)sock.GetSocketOption(SocketOptionLevel.Socket, SocketOptionName.Error);
}
catch (Exception e)
{
EDB.WriteLine("Failed to get sock options! (error: " + error + ")" + e);
}
if (error != 0)
{
EDB.WriteLine("SOCKET ERROR = " + error);
}
close();
}
}
}
public bool pubUpdate(string topic, List <string> pubs)
{
#if DEBUG
EDB.WriteLine("TopicManager is updating publishers for " + topic);
#endif
Subscription sub = null;
lock (subs_mutex)
{
if (shutting_down)
{
return(false);
}
foreach (Subscription s in subscriptions)
{
if (s.name != topic || s.IsDropped)
{
continue;
}
sub = s;
break;
}
}
if (sub != null)
{
return(sub.pubUpdate(pubs));
}
EDB.WriteLine("got a request for updating publishers of topic " + topic +
", but I don't have any subscribers to that topic.");
return(false);
}
private bool validateFailed(string method, string errorfmat, params object[] info)
{
#if XMLRPC_DEBUG
EDB.WriteLine("XML-RPC Call [{0}] {1} failed validation", method, string.Format(errorfmat, info));
#endif
return(false);
}
private void onConnectionDropped(Connection conn, Connection.DropReason reason)
{
EDB.WriteLine("TransportPublisherLink: onConnectionDropped -- " + reason);
if (dropping || conn != connection)
{
return;
}
lock (parent)
{
if (reason == Connection.DropReason.TransportDisconnect)
{
needs_retry = true;
next_retry = DateTime.Now.Add(retry_period);
if (retry_timer == null)
{
retry_period = TimeSpan.FromMilliseconds(100);
}
ROS.timer_manager.StartTimer(ref retry_timer, onRetryTimer,
(int)Math.Floor(retry_period.TotalMilliseconds), Timeout.Infinite);
}
else
{
if (reason == Connection.DropReason.HeaderError)
{
EDB.WriteLine("SOMETHING BE WRONG WITH THE HEADER FOR: " +
(parent != null ? parent.name : "unknown"));
}
drop();
}
}
}
internal bool advertiseService <MReq, MRes>(AdvertiseServiceOptions <MReq, MRes> ops) where MReq : IRosMessage, new() where MRes : IRosMessage, new()
{
lock (shutting_down_mutex)
{
if (shutting_down)
{
return(false);
}
}
lock (service_publications_mutex)
{
if (isServiceAdvertised(ops.service))
{
EDB.WriteLine("Tried to advertise a service that is already advertised in this node [{0}]", ops.service);
return(false);
}
if (ops.helper == null)
{
ops.helper = new ServiceCallbackHelper <MReq, MRes>(ops.srv_func);
}
ServicePublication <MReq, MRes> pub = new ServicePublication <MReq, MRes>(ops.service, ops.md5sum, ops.datatype, ops.req_datatype, ops.res_datatype, ops.helper, ops.callback_queue, ops.tracked_object);
service_publications.Add(pub);
}
XmlRpcValue args = new XmlRpcValue(), result = new XmlRpcValue(), payload = new XmlRpcValue();
args.Set(0, this_node.Name);
args.Set(1, ops.service);
args.Set(2, string.Format("rosrpc://{0}:{1}", network.host, connection_manager.TCPPort));
args.Set(3, xmlrpc_manager.uri);
master.execute("registerService", args, ref result, ref payload, true);
return(true);
}
internal bool lookupService(string name, ref string serv_host, ref int serv_port)
{
XmlRpcValue args = new XmlRpcValue(), result = new XmlRpcValue(), payload = new XmlRpcValue();
args.Set(0, this_node.Name);
args.Set(1, name);
if (!master.execute("lookupService", args, ref result, ref payload, false))
{
EDB.WriteLine("lookupService: Service unknown.");
return(false);
}
string serv_uri = payload.GetString();
if (serv_uri.Length == 0)
{
EDB.WriteLine("lookupService: Empty server URI returned from master");
return(false);
}
if (!network.splitURI(serv_uri, ref serv_host, ref serv_port))
{
EDB.WriteLine("lookupService: Bad service uri [{0}]", serv_uri);
return(false);
}
return(true);
}
private bool onMessageLength(Connection conn, byte[] buffer, int size, bool success)
{
ScopedTimer.Ping();
if (retry_timer != null)
{
ROS.timer_manager.RemoveTimer(ref retry_timer);
}
if (!success)
{
if (connection != null)
{
connection.read(4, onMessageLength);
}
return(true);
}
if (conn != connection || size != 4)
{
return(false);
}
int len = BitConverter.ToInt32(buffer, 0);
if (len > 1000000000)
{
EDB.WriteLine("TransportPublisherLink: 1 GB message WTF?!");
drop();
return(false);
}
connection.read(len, onMessage);
return(true);
}
private void onRetryTimer(object o)
{
#if DEBUG
EDB.WriteLine("TransportPublisherLink: onRetryTimer");
#endif
if (dropping)
{
return;
}
if (needs_retry && DateTime.Now.Subtract(next_retry).TotalMilliseconds < 0)
{
retry_period =
TimeSpan.FromSeconds((retry_period.TotalSeconds > 20) ? 20 : (2 * retry_period.TotalSeconds));
needs_retry = false;
TcpTransport old_transport = connection.transport;
string host = old_transport.connected_host;
int port = old_transport.connected_port;
TcpTransport transport = new TcpTransport();
if (transport.connect(host, port))
{
Connection conn = new Connection();
conn.initialize(transport, false, null);
initialize(conn);
ConnectionManager.Instance.addConnection(conn);
}
}
}
public bool initialize(Connection connection)
{
#if DEBUG
EDB.WriteLine("Init transport publisher link: " + parent.name);
#endif
this.connection = connection;
connection.DroppedEvent += onConnectionDropped;
if (connection.transport.getRequiresHeader())
{
connection.setHeaderReceivedCallback(onHeaderReceived);
IDictionary header = new Hashtable();
header["topic"] = parent.name;
header["md5sum"] = parent.md5sum;
header["callerid"] = this_node.Name;
header["type"] = parent.datatype;
header["tcp_nodelay"] = "1";
connection.writeHeader(header, onHeaderWritten);
}
else
{
connection.read(4, onMessageLength);
}
return(true);
}
public bool onConnectionHeaderReceived(Connection conn, Header header)
{
bool ret = false;
if (header.Values.Contains("topic"))
{
TransportSubscriberLink sub_link = new TransportSubscriberLink();
ret = sub_link.initialize(conn);
ret &= sub_link.handleHeader(header);
}
else if (header.Values.Contains("service"))
{
IServiceClientLink iscl = new IServiceClientLink();
ret = iscl.initialize(conn);
ret &= iscl.handleHeader(header);
}
else
{
EDB.WriteLine("got a connection for a type other than topic or service from [" + conn.RemoteString +
"]. Fail.");
return(false);
}
//EDB.WriteLine("CONNECTED [" + val + "]. WIN.");
return(ret);
}
public override void removeByID(UInt64 owner_id)
{
setupTLS();
IDInfo idinfo;
lock (id_info_mutex)
{
if (!id_info.ContainsKey(owner_id))
{
return;
}
idinfo = id_info[owner_id];
}
if (idinfo.id == tls.calling_in_this_thread)
{
removeemall(owner_id);
}
else
{
#if DEBUG
EDB.WriteLine("removeByID w/ WRONG THREAD ID");
#endif
removeemall(owner_id);
}
}
public void addPollThreadListener(Action poll)
{
EDB.WriteLine("Adding pollthreadlistener " + poll.Target + ":" + poll.Method);
lock (signal_mutex)
{
signals.Add(new Poll_Signal(poll));
}
signal();
}
public bool initialize(Connection connection)
{
if (parent != null)
{
EDB.WriteLine("Init transport subscriber link: " + parent.Name);
}
this.connection = connection;
connection.DroppedEvent += onConnectionDropped;
return(true);
}
public void func <T>(T msg) where T : IRosMessage, new()
{
if (Event != null)
{
Event(msg);
}
else
{
EDB.WriteLine("EVENT IS NULL");
}
}
public void setNoDelay(bool nd)
{
try
{
sock.NoDelay = nd;
}
catch (Exception e)
{
EDB.WriteLine(e);
}
}
//public void requestTopicCallback([In] [Out] IntPtr parms, [In] [Out] IntPtr result)
public void requestTopicCallback(XmlRpcValue parm, XmlRpcValue res)
{
//XmlRpcValue res = XmlRpcValue.Create(ref result)
// , parm = XmlRpcValue.Create(ref parms);
//result = res.instance;
if (!requestTopic(parm[1].Get <string>(), parm[2], ref res))
{
const string error = "Unknown error while handling XmlRpc call to requestTopic";
EDB.WriteLine(error);
XmlRpcManager.Instance.responseInt(0, error, 0)(res);
}
}
public bool connect(string host, int port)
{
sock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
connected_host = host;
connected_port = port;
setNonBlocking();
IPAddress IPA = null;
if (!IPAddress.TryParse(host, out IPA))
{
foreach (IPAddress ipa in Dns.GetHostAddresses(host).Where(ipa => !ipa.ToString().Contains(":")))
{
IPA = ipa;
break;
}
if (IPA == null)
{
close();
EDB.WriteLine("Couldn't resolve host name [{0}]", host);
return(false);
}
}
if (IPA == null)
{
return(false);
}
IPEndPoint ipep = new IPEndPoint(IPA, port);
LocalEndPoint = ipep;
if (!sock.ConnectAsync(new SocketAsyncEventArgs {
RemoteEndPoint = ipep
}))
{
return(false);
}
cached_remote_host = "" + host + ":" + port + " on socket " + sock;
while (ROS.ok && !sock.Connected)
{
}
if (!ROS.ok || !initializeSocket())
{
return(false);
}
return(true);
}
public override void drop()
{
dropping = true;
connection.drop(Connection.DropReason.Destructing);
if (parent != null)
{
parent.removePublisherLink(this);
}
else
{
EDB.WriteLine("TransportPublisherLink met an untimely demise.");
}
}
public TcpTransport(PollSet pollset, int flags) : this()
{
if (pollset != null)
{
poll_set = pollset;
poll_set.DisposingEvent += close;
}
else
{
EDB.WriteLine("Uh oh! Null pollset in tcptransport ctor");
}
this.flags = flags;
}
private void threadFunc()
{
while (!shutting_down)
{
signal();
Thread.Sleep(ROS.WallDuration);
if (shutting_down)
{
return;
}
}
EDB.WriteLine("PollManager thread IS FREE");
}
public bool handleHeader(Header header)
{
if (!header.Values.Contains("topic"))
{
string msg = "Header from subscriber did not have the required element: topic";
EDB.WriteLine(msg);
connection.sendHeaderError(ref msg);
return(false);
}
string name = (string)header.Values["topic"];
string client_callerid = (string)header.Values["callerid"];
Publication pt = TopicManager.Instance.lookupPublication(name);
if (pt == null)
{
string msg = "received a connection for a nonexistent topic [" + name + "] from [" +
connection.transport + "] [" + client_callerid + "]";
EDB.WriteLine(msg);
connection.sendHeaderError(ref msg);
return(false);
}
string error_message = "";
if (!pt.validateHeader(header, ref error_message))
{
connection.sendHeaderError(ref error_message);
EDB.WriteLine(error_message);
return(false);
}
destination_caller_id = client_callerid;
connection_id = ConnectionManager.Instance.GetNewConnectionID();
name = pt.Name;
parent = pt;
lock (parent)
{
max_queue = parent.MaxQueue;
}
IDictionary m = new Hashtable();
m["type"] = pt.DataType;
m["md5sum"] = pt.Md5sum;
m["message_definition"] = pt.MessageDefinition;
m["callerid"] = this_node.Name;
m["latching"] = pt.Latch;
connection.writeHeader(m, onHeaderWritten);
pt.addSubscriberLink(this);
#if DEBUG
EDB.WriteLine("Finalize transport subscriber link for " + name);
#endif
return(true);
}
/// <summary>
/// </summary>
/// <param name="method"></param>
/// <param name="request">Full request to send to the master </param>
/// <param name="response">Full response including status code and status message. Initially empty.</param>
/// <param name="payload">Location to store the actual data requested, if any.</param>
/// <param name="wait_for_master">If you recieve an unseccessful status code, keep retrying.</param>
/// <returns></returns>
public static bool execute(string method, XmlRpcValue request, ref XmlRpcValue response, ref XmlRpcValue payload,
bool wait_for_master)
{
try
{
DateTime startTime = DateTime.Now;
string master_host = host;
int master_port = port;
//EDB.WriteLine("Trying to connect to master @ " + master_host + ":" + master_port);
XmlRpcClient client = XmlRpcManager.Instance.getXMLRPCClient(master_host, master_port, "/");
bool printed = false;
bool ok = true;
while (!client.IsConnected && !ROS.shutting_down && !XmlRpcManager.Instance.shutting_down ||
!(ok = client.Execute(method, request, response) && XmlRpcManager.Instance.validateXmlrpcResponse(method, response, ref payload)))
{
if (!wait_for_master)
{
XmlRpcManager.Instance.releaseXMLRPCClient(client);
return(false);
}
if (!printed)
{
EDB.WriteLine("[{0}] FAILED TO CONTACT MASTER AT [{1}:{2}]. {3}", method, master_host,
master_port, (wait_for_master ? "Retrying..." : ""));
printed = true;
}
if (retryTimeout.TotalSeconds > 0 && DateTime.Now.Subtract(startTime) > retryTimeout)
{
EDB.WriteLine("[{0}] Timed out trying to connect to the master after [{1}] seconds", method,
retryTimeout.TotalSeconds);
XmlRpcManager.Instance.releaseXMLRPCClient(client);
return(false);
}
Thread.Sleep(10);
}
if (ok && !firstsucces)
{
firstsucces = true;
//EDB.WriteLine(string.Format("CONNECTED TO MASTER AT [{0}:{1}]", master_host, master_port));
}
XmlRpcManager.Instance.releaseXMLRPCClient(client);
return(true);
}
catch (Exception e)
{
Console.WriteLine(e);
}
return(false);
}
public static void Init(string n, IDictionary remappings, int options)
{
Name = n;
bool disable_anon = false;
if (remappings.Contains("__name"))
{
Name = (string)remappings["__name"];
disable_anon = true;
}
if (remappings.Contains("__ns"))
{
Namespace = (string)remappings["__ns"];
}
if (Namespace == "")
{
Namespace = "/";
}
long walltime = DateTime.Now.Subtract(Process.GetCurrentProcess().StartTime).Ticks;
names.Init(remappings);
if (Name.Contains("/"))
{
throw new Exception("NAMES CANT HAVE SLASHES, WENCH!");
}
if (Name.Contains("~"))
{
throw new Exception("NAMES CANT HAVE SQUIGGLES, WENCH!");
}
try
{
Name = names.resolve(Namespace, Name);
}
catch (Exception e)
{
EDB.WriteLine(e);
}
if ((options & (int)InitOption.AnonymousName) == (int)InitOption.AnonymousName && !disable_anon)
{
int lbefore = Name.Length;
Name += "_" + walltime;
if (Name.Length - lbefore > 201)
{
Name = Name.Remove(lbefore + 201);
}
}
}
/// <summary>
/// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources.
/// </summary>
public void Dispose()
{
lock (busyMutex)
if (refs != 0)
{
EDB.WriteLine("warning: XmlRpcClient disposed with " + refs + " refs held");
}
lock (client_lock)
{
if (client != null)
{
client.Dispose();
client = null;
}
}
}
