public Transformer(bool interpolating = true, ulong ct = (ulong)DEFAULT_CACHE_TIME)
{
frameIDs["NO_PARENT"] = 0;
frameids_reverse[0] = "NO_PARENT";
#if FOR_UNITY
#if UNITY_EDITOR
if (UnityEditor.EditorApplication.isPlaying)
{
#endif
if (!ROS.isStarted())
{
lock (instances)
instances.Add(this);
}
else
{
#endif
InitNH();
#if FOR_UNITY
}
#if UNITY_EDITOR
}
#endif
#endif
this.interpolating = interpolating;
cache_time = ct;
}
public RobotDescriptionParser(string robot_description_param, string tf_prefix = null, Dictionary <string, string> hardcoded_package_paths = null)
{
this.robot_description_param = robot_description_param;
this.tf_prefix = tf_prefix;
this.hardcoded_package_paths = hardcoded_package_paths;
new Thread(() => {
while (!ROS.isStarted() || !ROS.ok)
{
if (ROS.shutting_down)
{
break;
}
Thread.Sleep(100);
}
if (!ROS.shutting_down)
{
nh = ROS.GlobalNodeHandle;
// nh = new NodeHandle();
Load();
}
})
{
IsBackground = true
}.Start();
}
private void waitfunc(string TopicName)
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
SetupTopic(TopicName);
}
private void waitfunc(string Namespace)
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
SetupNamespace(Namespace);
}
private void waitfunc(string ignored)
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
SetupIgnore(ignored);
}
private void waitfunc(string topic)
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
Thread.Sleep(1000);
SetupTopic(topic);
}
private void waitThenSubscribe(string topic)
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
Thread.Sleep(1000);
SubscribeToMap(topic);
}
private static void _startROS()
{
if (!ROS.isStarted() && roslock.WaitOne())
{
// Make sure we are still the first caller to initialize ros, in case we blocked waiting for the lock
if (!ROS.isStarted())
{
ROS.Init(new string[0], "unity_test_" + DateTime.Now.Ticks);
tf.net.Transformer.LateInit();
}
roslock.Set();
}
}
private void Init()
{
while (!ROS.isStarted())
{
Thread.Sleep(100);
}
if (nodeHandle == null)
{
nodeHandle = new NodeHandle();
}
if (subscriber == null)
{
subscriber = nodeHandle.subscribe <Messages.rosgraph_msgs.Log>("/rosout_agg", 100, Callback);
}
}
void Update()
{
if (ROS.isStarted() && ROS.ok && connectedToMaster)
{
status = ROSStatus.Connected;
}
else
if (ROS.shutting_down || !ROS.isStarted() || !ROS.ok)
{
status = ROSStatus.Disconnected;
}
else
{
status = ROSStatus.Connecting;
}
}
private void waitThenSubscribe()
{
while (true)
{
Thread.Sleep(100);
lock (this)
if (ROS.shutting_down || ROS.isStarted())
{
break;
}
}
lock (this)
if (ROS.shutting_down)
{
return;
}
SubscribeToImage(__topic);
}
static ROSMonoBehavior()
{
rosmanager = new ROSManager();
#if UNITY_EDITOR
UnityEditor.EditorApplication.playmodeStateChanged = () =>
{
string state = "";
if (EditorApplication.isPlaying)
{
state += "playing";
}
if (EditorApplication.isPaused)
{
state += " paused";
}
if (EditorApplication.isCompiling)
{
state += " compiling";
}
state = state.Trim(' ');
Debug.LogWarning("PlayMode == " + state);
if (!EditorApplication.isPlaying && !EditorApplication.isPaused)
{
ROS.Unfreeze();
if (ROS.ok || ROS.isStarted())
{
ROSManager.StopROS();
}
}
else if (EditorApplication.isPlaying)
{
if (!EditorApplication.isPaused)
{
ROS.Unfreeze();
rosmanager.StartROS(null, null);
}
else
{
ROS.Freeze();
}
}
};
#endif
}
private void Init()
{
if (!ROS.isStarted())
{
if (waitforinit == null)
{
string workaround = Topic;
waitforinit = new Thread(() => waitfunc(workaround));
}
if (!waitforinit.IsAlive)
{
waitforinit.Start();
}
}
else
{
SetupTopic(Topic);
}
}
private void DrawMap()
{
if (!ROS.isStarted())
{
if (waitingThread == null)
{
string topicString = Topic;
waitingThread = new Thread(() => waitThenSubscribe(topicString));
}
if (!waitingThread.IsAlive)
{
waitingThread.Start();
}
}
else
{
SubscribeToMap(Topic);
}
}
public static void StopROS(Action callback = null)
{
if (ROS.isStarted() && !ROS.shutting_down && !instance.stopping)
{
// instance.status = ROSStatus.Disconnected;
instance.starting = false;
instance.stopping = true;
while (nodes.Count > 0)
{
NodeHandle node = nodes.Dequeue();
node.shutdown();
node.Dispose();
}
Debug.Log("stopping ROS");
ROS.shutdown();
ROS.waitForShutdown();
}
if (callback != null)
{
callback();
}
}
private void Init()
{
lock (this)
{
if (!ROS.isStarted())
{
if (waitforinit == null)
{
string workaround = Namespace;
waitforinit = new Thread(() => waitfunc(workaround));
}
if (!waitforinit.IsAlive)
{
waitforinit.Start();
}
}
else
{
SetupNamespace(Namespace);
}
}
}
private void Init()
{
lock (this)
{
if (!ROS.isStarted())
{
if (waitforinit == null)
{
string workaround = IgnoredStrings;
waitforinit = new Thread(() => waitfunc(workaround));
}
if (!waitforinit.IsAlive)
{
waitforinit.Start();
}
}
else
{
SetupIgnore(IgnoredStrings);
}
}
}
private void DrawImage()
{
lock (this)
{
if (!ROS.isStarted())
{
if (waitingThread == null)
{
waitingThread = new Thread(() => waitThenSubscribe());
}
if (!waitingThread.IsAlive)
{
waitingThread.Start();
}
return;
}
}
if (!ROS.isStarted() || ROS.shutting_down)
{
return;
}
SubscribeToImage(Topic);
}
IEnumerator WaitForInit()
{
while (!ROS.isStarted() && !ROS.ok && !stopping && ROS.GlobalNodeHandle == null)
{
yield return(null);
}
#if UNITY_STANDALONE_OSX || UNITY_STANDALONE_LINUX || UNITY_STANDALONE_LINUX_API
yield return(new WaitForSeconds(0.1f));
if (ROS.GlobalNodeHandle == null)
{
Debug.LogError("Why is this null!?");
}
#endif
// XmlRpcUtil.SetLogLevel(XmlRpcUtil.XMLRPC_LOG_LEVEL.ERROR);
if (ROS.ok && !stopping)
{
lock ( instanceLock )
{
starting = false;
initComplete = true;
}
// status = ROSStatus.Connected;
Debug.Log("ROS Init successful");
lock ( callbackLock )
{
while (callbacks != null && callbacks.Count > 0)
{
Action action = callbacks.Dequeue();
new Thread(new ThreadStart(action)).Start();
// callbacks.Dequeue () ();
}
}
}
}
public static void StopROS()
{
if (ROS.isStarted() && !ROS.shutting_down && roslock.WaitOne())
{
if (ROS.isStarted() && !ROS.shutting_down)
{
Debug.Log("ROSManager is shutting down");
ROS.shutdown();
}
roslock.Set();
}
ROS.waitForShutdown();
#if LOG_TO_FILE
lock (loggerlock)
{
Application.logMessageReceived -= Application_logMessageReceived;
if (logwriter != null)
{
logwriter.Close();
logwriter = null;
}
}
#endif
}
//ROS
private void UserControl_Loaded(object sender, RoutedEventArgs e)
{
new Thread(() =>
{
while (!ROS.isStarted())
{
Thread.Sleep(200);
}
node = new NodeHandle();
//setup up temporary arrays to make setup clean
Slider[][] sliders = new[]
{ new [] { MC1_Brigh_Sl, MC1_Cont_Sl, MC1_Exp_Sl, MC1_Gain_Sl, MC1_Sat_Sl, MC1_WBT_Sl, MC1_Foc_Sl },
new [] { RC_Brigh_Sl, RC_Cont_Sl, RC_Exp_Sl, RC_Gain_Sl, RC_Sat_Sl, RC_WBT_Sl, RC_Foc_Sl },
new [] { MC3_Brigh_Sl, MC3_Cont_Sl, MC3_Exp_Sl, MC3_Gain_Sl, MC3_Sat_Sl, MC3_WBT_Sl, MC3_Foc_Sl },
new [] { MC4_Brigh_Sl, MC4_Cont_Sl, MC4_Exp_Sl, MC4_Gain_Sl, MC4_Sat_Sl, MC4_WBT_Sl, MC4_Foc_Sl } };
Label[][] labels = new[]
{ new[] { MC1_Brigh_Lvl, MC1_Cont_Lvl, MC1_Exp_Lvl, MC1_Gain_Lvl, MC1_Sat_Lvl, MC1_WBT_Lvl, MC1_Foc_Lvl },
new[] { RC_Brigh_Lvl, RC_Cont_Lvl, RC_Exp_Lvl, RC_Gain_Lvl, RC_Sat_Lvl, RC_WBT_Lvl, RC_Foc_Lvl },
new[] { MC3_Brigh_Lvl, MC3_Cont_Lvl, MC3_Exp_Lvl, MC3_Gain_Lvl, MC3_Sat_Lvl, MC3_WBT_Lvl, MC3_Foc_Lvl },
new[] { MC4_Brigh_Lvl, MC4_Cont_Lvl, MC4_Exp_Lvl, MC4_Gain_Lvl, MC4_Sat_Lvl, MC4_WBT_Lvl, MC4_Foc_Lvl } };
int[] defaults = new int[] { 128, 33, 100, 64, 32, 5315, 16 };
string[] info = new[] { "camera0", "camera1", "camera2", "camera3" };
//end setup
//setup persistent array of slider stuff for storage
SUBS = new[] { new SliderStuff[6], new SliderStuff[6], new SliderStuff[6], new SliderStuff[6] };
initials = new[] { new cm[6], new cm[6], new cm[6], new cm[6] };
pub_exposureauto = new Publisher <m.Int32> [4];
//pub_wbtauto = new Publisher<m.Int32>[4];
pub_focusauto = new Publisher <m.Int32> [4];
////// INITIALIZE THIS CAMS PUB AND SUB
pub = new Publisher <cm> [4];
sub = new Subscriber <cm>[] { node.subscribe <cm>("/camera0/sliders_info", 1, cb0),
node.subscribe <cm>("/camera1/sliders_info", 1, cb1),
node.subscribe <cm>("/camera2/sliders_info", 1, cb2),
node.subscribe <cm>("/camera3/sliders_info", 1, cb3) };
for (int i = 0; i < info.Length; i++)
{
//pub_exposureauto[i] = node.advertise<m.Int32>("camera" + i + "/exposureauto", 1);
//pub_wbtauto[i] = node.advertise<m.Int32>("camera" + i + "/wbtauto", 1);
//pub_focusauto[i] = node.advertise<m.Int32>("camera" + i + "/focusauto", 1);
pub[i] = node.advertise <cm>(info[i] + "/sliders", 1);
for (int j = 0; j < 6; j++)
{
Dispatcher.Invoke(new Action(() =>
{
SUBS[i][j] = new SliderStuff(node, j, info[i], sliders[i][j], labels[i][j]);
SUBS[i][j].setFire(fire);
SUBS[i][j].Value = defaults[j];
if (initials[i][j] != null)
{
SUBS[i][j].callback(initials[i][j]);
}
}));
}
}
}).Start();
}