/*#if UNITY_EDITOR
* internal bool wrong_parent_warned = false;
*
* public void CheckHierarchy(bool needToCheck = true)
* {
* if (transform.parent == null || transform.parent.gameObject.GetComponent<TfVisualizer>() == null)
* {
* if (!wrong_parent_warned || !needToCheck)
* {
* EditorUtility.DisplayDialog("Invalid hierarchy", @"This script MUST be attached to a component that is a child of TFTree.
*
* Example:
* TFTree
* TFFrame (required but unrelated)
* RobotTemplate (with this script added to it)
* LaserView
* OdometryView
* (
* AND each other thing to copy to
* ALL of the robots related to this
* subcriptionmanager
* )", "I understand");
* wrong_parent_warned = true;
* }
* }
* }
#endif*/
void Start()
{
/*#if UNITY_EDITOR
* CheckHierarchy(false);
#endif*/
rosmanager.StartROS(this, () =>
{
int numRobots;
if (!int.TryParse(Robot_Count, out numRobots) && Robot_Count.Length != 0)
{
if (!Param.get(Robot_Count, ref numRobots))
{
numRobots = 1;
EDB.WriteLine("Failed to treat NumberOfRobots: {0} as a rosparam name. Using 1 robot as the default", Robot_Count);
}
}
TfTreeManager.Instance.AddListener(vis =>
{
GameObject parent = new GameObject();
parent.name = "Templated Robots";
parent.transform.parent = vis.transform.root;
for (int num = First_Index; num < First_Index + numRobots; num++)
{
GameObject go = new GameObject();
go.name = NameSpace_Prefix + num + " (" + GetType().Name + ")";
go.transform.parent = parent.transform;
foreach (Transform prefabTF in transform)
{
//Maybe put this outside to remove unnecessary loops
foreach (Component script in prefabTF.GetComponents(typeof(MonoBehaviour)))
{
if (!parentScripts.Contains(script))
{
parentScripts.Add(script);
}
}
GameObject prefab = Instantiate(prefabTF.gameObject, parent.transform, false);
//add all instantiated objects scripts to child scripts
foreach (Component script in prefab.GetComponents(typeof(MonoBehaviour)))
{
childScripts.Add(script);
}
prefab.transform.parent = go.transform;
prefab.name = prefabTF.name;
prefab.SendMessage("setNamespace", NameSpace_Prefix + num, SendMessageOptions.DontRequireReceiver);
}
}
//after instantiation disable old prefabs to prevent conflicts with default agents
foreach (Transform prefabTf in transform)
{
prefabTf.gameObject.SetActive(false);
}
});
});
}
private void callback(Messages.rosgraph_msgs.Log msg)
{
string teststring = string.Format("{0}\n{1}\n{2}\n{3}\n{4}\n{5}", msg.level, msg.msg, msg.name, msg.file, msg.function, msg.line);
if (ignoredStrings.Count > 0 && ignoredStrings.Any(teststring.Contains))
{
return;
}
rosoutString rss = new rosoutString((1.0 * msg.header.stamp.data.sec + (1.0 * msg.header.stamp.data.nsec) / 1000000000.0),
msg.level,
msg.msg,
msg.name,
msg.file,
msg.function,
"" + msg.line);
Dispatcher.Invoke(new Action(() =>
{
rosoutdata.Add(rss);
// remove any messages that are more than a minute NEWER than the JUST RECEIVED log message
List <rosoutString> tooNew = rosoutdata.Where(s => s.stamp > rss.stamp + 60).ToList();
if (tooNew.Count > 0)
{
EDB.WriteLine("Dropping " + tooNew.Count + " rosout messages FROM THE FUTURE");
}
foreach (rosoutString rs in tooNew)
{
rosoutdata.Remove(rs);
}
if (rosoutdata.Count > 1000)
{
rosoutdata.RemoveAt(0);
}
abraCadabra.InvalidateMeasure();
//auto-sticky scrolling IFF the vertical scrollbar is at its maximum or minimum
if (VisualTreeHelper.GetChildrenCount(abraCadabra) > 0)
{
var border = VisualTreeHelper.GetChild(abraCadabra, 0) as Decorator;
if (border != null)
{
var scroll = border.Child as ScrollViewer;
if (scroll != null && !scroll.IsMouseCaptured)
{
if (scroll.VerticalOffset <= 0 || scroll.VerticalOffset >= scroll.ScrollableHeight - 2)
{
abraCadabra.ScrollIntoView(rss);
}
}
else
{
Console.WriteLine("yay");
}
}
}
}));
}
protected override void Dispose(bool disposing)
{
if (!disposed)
{
EDB.WriteLine("Killing socket w/ FD=" + FD + (attemptedConnectionEndpoint == null ? "" : "\tTO REMOTE HOST\t" + attemptedConnectionEndpoint));
if (Get(FD) != null)
{
_socklist.Remove(FD);
}
disposed = true;
_freelist.Add(FD);
base.Dispose(disposing);
}
}
protected override void Dispose(bool disposing)
{
lock (this)
{
if (!disposed)
{
EDB.WriteLine("Killing socket w/ FD=" + _fakefd + (attemptedConnectionEndpoint == null ? "" : "\tTO REMOTE HOST\t" + attemptedConnectionEndpoint));
disposed = true;
remove(_fakefd);
_freelist.Add(_fakefd);
_fakefd = 0;
base.Dispose(disposing);
}
}
}
public void Dispose()
{
lock (this)
{
if (!disposed && _fakefd != 0)
{
#if DEBUG
EDB.WriteLine("Killing socket w/ FD=" + _fakefd + (attemptedConnectionEndpoint == null ? "" : "\tTO REMOTE HOST\t" + attemptedConnectionEndpoint));
#endif
disposed = true;
_freelist.Add(_fakefd);
_fakefd = 0;
realsocket.Close();
realsocket = null;
}
}
}
public bool SafePoll(int timeout, ns.SelectMode sm)
{
bool res = false;
try
{
if (!disposed)
{
res = realsocket.Poll(timeout, sm);
}
}
catch (ns.SocketException e)
{
EDB.WriteLine(e);
res = !disposed && sm == ns.SelectMode.SelectError;
}
return(res);
}
private void _set(Config config)
{
string reason = "";
if (!set(config, ref reason))
{
lock (this)
if (_cli != null)
{
_cli.shutdown();
_cli = null;
}
if (!set(config, ref reason))
{
EDB.WriteLine("SET FAILED\n" + reason);
}
}
}
public void SubscribeForUpdates()
{
if (configSub != null)
{
configSub.shutdown();
}
if (descSub != null)
{
descSub.shutdown();
}
string configtop = names.resolve(name, "parameter_updates");
string paramtop = names.resolve(name, "parameter_descriptions");
try
{
configSub = nh.subscribe <Config>(configtop, 1, ConfigCallback);
descSub = nh.subscribe <ConfigDescription>(paramtop, 1, DescriptionCallback);
}
catch (InvalidCastException ice)
{
EDB.WriteLine(ice);
}
}
public MainWindow()
{
new Thread(() =>
{
ROS.Init(new string[0], "dynamic_reconfigure_sharp_" + Environment.MachineName);
nh = new NodeHandle();
Dispatcher.Invoke(new Action(() => { ConnecitonLabel.Content = "Connected"; }));
}).Start();
try
{
InitializeComponent();
System.Diagnostics.PresentationTraceSources.DataBindingSource.Switch.Level = System.Diagnostics.SourceLevels.Critical;
TargetBox.ItemsSource = knownConfigurations.Keys;
knownConfigurations.Add("-", null);
TargetBox.SelectedIndex = 0;
}
catch (Exception e)
{
EDB.WriteLine(e);
Close();
}
topicPoller = new Thread(() =>
{
while (ROS.ok && !ROS.shutting_down)
{
TopicInfo[] topics = new TopicInfo[0];
master.getTopics(ref topics);
string[] nodes = new string[0];
master.getNodes(ref nodes);
List <string> prevlist = new List <string>(knownConfigurations.Keys);
List <string> additions = new List <string>();
foreach (TopicInfo ti in topics)
{
if (ti.data_type == "dynamic_reconfigure/Config")
{
string prefix = ti.name.Replace("/parameter_updates", "");
if (!knownConfigurations.ContainsKey(prefix))
{
additions.Add(prefix);
}
else
{
prevlist.Remove(prefix);
}
}
}
lock (this)
{
if (!ROS.ok || ROS.shutting_down)
{
return;
}
foreach (string prefix in additions)
{
string pfx = prefix;
Dispatcher.Invoke(new Action(() =>
{
knownConfigurations.Add(pfx, null);
}), new TimeSpan(0, 0, 0, 1));
}
}
Dispatcher.Invoke(new Action(TargetBox.Items.Refresh));
foreach (string s in prevlist)
{
if (!s.Equals("-"))
{
string pfx = s;
Dispatcher.Invoke(new Action(() =>
{
if (reconfigureview != null && s.Equals(reconfigureview.Namespace))
{
reconfigureview.Namespace = null;
}
if (TargetBox.SelectedItem != null && ((string)TargetBox.SelectedItem).Equals(pfx))
{
TargetBox.SelectedIndex = 0;
}
lock (this)
{
knownConfigurations.Remove(pfx);
}
}), new TimeSpan(0, 0, 0, 1));
}
}
Dispatcher.Invoke(new Action(TargetBox.Items.Refresh));
if (reconfigureview == null && nh != null)
{
Dispatcher.Invoke(new Action(() =>
{
string target = null;
if (TargetBox.SelectedItem != null && !String.Equals(TargetBox.SelectedItem.ToString(), "-"))
{
target = TargetBox.SelectedItem.ToString();
}
reconfigureview = new DynamicReconfigurePage(nh, target);
PageContainer.Children.Add(reconfigureview);
}));
}
Thread.Sleep(500);
}
});
topicPoller.Start();
}
public override string ToString()
{
return(ODB.ToString() + EDB.ToString() + FANAVF.ToString() + TotalCAP.ToString()
+ InputPWR.ToString());
}
