public DynamicReconfigureInterface(NodeHandle n, string name, int timeout = 0)
{
nh = n;
this.name = name;
this.set_service_name = names.resolve(name, "set_parameters");
this.timeout = timeout;
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
ROS.Init(new string[0], NODE_NAME+DateTime.Now.Ticks);
nodeHandle = new NodeHandle();
new Thread(new ThreadStart(() =>
{
Random r = new Random();
while (ROS.ok)
{
TwoInts.Request req = new TwoInts.Request() { a = r.Next(100), b = r.Next(100) };
TwoInts.Response resp = new TwoInts.Response();
DateTime before = DateTime.Now;
bool res = nodeHandle.serviceClient<TwoInts.Request, TwoInts.Response>("/add_two_ints").call(req, ref resp);
TimeSpan dif = DateTime.Now.Subtract(before);
Dispatcher.Invoke(new Action(() =>
{
string str = "";
if (res)
str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
else
str = "call failed after\n";
str += Math.Round(dif.TotalMilliseconds,2) + " ms";
math.Content = str;
}));
Thread.Sleep(500);
}
})).Start();
}
protected override void Initialize()
{
quad = new Quad[6];
quad[0] = new Quad(Vector3.Backward, Vector3.Backward, Vector3.Up, 2, 2);
quad[1] = new Quad(Vector3.Left, Vector3.Left, Vector3.Up, 2, 2);
quad[2] = new Quad(Vector3.Right, Vector3.Right, Vector3.Up, 2, 2);
quad[3] = new Quad(Vector3.Forward, Vector3.Forward, Vector3.Up, 2, 2);
quad[4] = new Quad(Vector3.Down, Vector3.Down, Vector3.Right, 2, 2);
quad[5] = new Quad(Vector3.Up, Vector3.Up, Vector3.Left, 2, 2);
nh = new NodeHandle();
imgSub = nh.subscribe<Messages.sensor_msgs.Image>("/camera/rgb/image_rect_color", 1, (img) =>
{
if (padlock.WaitOne(10))
{
if (next_texture == null)
{
next_texture = new Texture2D(GraphicsDevice, (int) img.width, (int) img.height);
}
util.UpdateImage(GraphicsDevice, img.data, new Size((int)img.width, (int)img.height), ref next_texture, img.encoding.data);
padlock.ReleaseMutex();
}
});
base.Initialize();
}
public DiagramNode()
{
for(int i = 0 ; i < 8 ; i++){
handles[i] = new NodeHandle(this , i);
}
uuid = System.Guid.NewGuid ().ToString ("N");
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "wpf_talker");
nh = new NodeHandle();
pub = nh.advertise<Messages.std_msgs.String>("/chatter", 1, false);
pubthread = new Thread(() =>
{
int i = 0;
Messages.std_msgs.String msg;
while (ROS.ok && !closing)
{
msg = new Messages.std_msgs.String("foo " + (i++));
pub.publish(msg);
Dispatcher.Invoke(new Action(() =>
{
l.Content = "Sending: " + msg.data;
}),new TimeSpan(0,0,1));
Thread.Sleep(100);
}
});
pubthread.Start();
}
private static void Main(string[] args)
{
ROS.Init(args, "Listener");
NodeHandle node = new NodeHandle();
Subscriber<m.String> Subscriber = node.subscribe<m.String>("/chatter", 1, chatterCallback);
ROS.waitForShutdown();
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
ROS.ROS_MASTER_URI = "http://10.0.2.88:11311";
ROS.ROS_HOSTNAME = "10.0.2.152";
ROS.Init(new string[0], NODE_NAME);
nodeHandle = new NodeHandle();
//server = nodeHandle.advertiseService<Messages.roscpp_tutorials.TwoInts, Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints", addition);
client = nodeHandle.serviceClient<Messages.roscpp_tutorials.TwoInts.Request, Messages.roscpp_tutorials.TwoInts.Response>("/add_two_ints");
new Thread(new ThreadStart(() =>
{
Random r = new Random();
while (!ROS.shutting_down)
{
TwoInts.Request req = new TwoInts.Request() { a = r.Next(100), b = r.Next(100) };
TwoInts.Response resp = new TwoInts.Response();
if (client.call(req, ref resp))
Dispatcher.Invoke(new Action(() =>
{
math.Content = "" + req.a + " + " + req.b + " = " + resp.sum;
}));
Thread.Sleep(500);
}
})).Start();
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
ROS.Init(new string[0], NODE_NAME);
nodeHandle = new NodeHandle();
server = nodeHandle.advertiseService<TwoInts.Request, TwoInts.Response>("/add_two_ints", addition);
}
public DynamicReconfigurePage(NodeHandle n, string name) : this()
{
nh = n;
Loaded += (sender, args) =>
{
Namespace = name;
};
}
public void startListening(NodeHandle nh)
{
updater = new DispatcherTimer() { Interval = new TimeSpan(0, 0, 0, 0, 100) };
updater.Tick += Link;
updater.Start();
sub = nh.subscribe<m.Int32>("/camera1/tilt_info", 1, callback);
pub = nh.advertise<m.Int32>("/camera1/tilt", 1);
}
static void Main(string[] args)
{
ROS.Init(args, "tf_example");
NodeHandle nh = new NodeHandle();
ROS.Info("This node will create a Transformer to compare lookup results between four source/target frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo");
tfer = new Transformer(false);
#region tf_echo results
/*
* tf_echo camera_link camera_rgb_frame
* (0.0,-0.045,0.0)
* (0,0,0,1)
*/
emTransform result1 = new emTransform() {basis = new emQuaternion(0, 0, 0, 1), origin = new emVector3(0, -0.045, 0), child_frame_id = "camera_rgb_frame", frame_id = "camera_link"};
/*
* tf_echo camera_link camera_rgb_optical_frame
* (0.0,-0.045,0.0)
* (-0.5,0.5,-0.5,0.5)
*/
emTransform result2 = new emTransform() { basis = new emQuaternion(-0.5, 0.5, -0.5, 0.5), origin = new emVector3(0.0, -0.045, 0.0), child_frame_id = "camera_rgb_optical_frame", frame_id = "camera_link" };
/*
* tf_echo camera_rgb_frame camera_depth_frame
* (0.0,0.25,0.0)
* (0,0,0,1)
*/
emTransform result3 = new emTransform() { basis = new emQuaternion(0,0,0,1), origin = new emVector3(0.0, 0.025, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_frame" };
/*
* tf_echo camera_rgb_optical_frame camera_depth_frame
* (-0.25,0.0,0.0)
* (0.5,-0.5,0.5,0.5)
*/
emTransform result4 = new emTransform() { basis = new emQuaternion(0.5, -0.5, 0.5, 0.5), origin = new emVector3(-0.025, 0.0, 0.0), child_frame_id = "camera_depth_frame", frame_id = "camera_rgb_optical_frame" };
#endregion
emTransform test1 = null, test2 = null, test3 = null, test4 = null;
do
{
if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
test1 = testLookup(result1);
if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
test2 = testLookup(result2);
if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
test3 = testLookup(result3);
if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
test4 = testLookup(result4);
Thread.Sleep(1000);
} while (!string.Equals(result1.ToString(), test1.ToString()) || !string.Equals(result2.ToString(), test2.ToString()) || !string.Equals(result3.ToString(), test3.ToString()) || !string.Equals(result4.ToString(), test4.ToString()));
Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
Console.ReadLine();
ROS.shutdown();
}
public MainWindow()
{
InitializeComponent();
ROS.Init(new string[0], "wpf_listener");
nh = new NodeHandle();
sub = nh.subscribe<Messages.std_msgs.String>("/chatter", 10, subCallback);
}
public RosoutDebug(NodeHandle n, string IgnoredStrings)
: this()
{
nh = n;
Loaded += (sender, args) =>
{
this.IgnoredStrings = IgnoredStrings;
};
}
public override void spin(CallbackQueue callbackInterface)
{
if (callbackInterface == null)
callbackInterface = ROS.GlobalCallbackQueue;
NodeHandle spinnerhandle = new NodeHandle();
while (spinnerhandle.ok)
{
callbackInterface.callAvailable(ROS.WallDuration);
}
}
private void Window_Loaded(object sender, RoutedEventArgs e)
{
new Thread(() =>
{
// ROS stuff
ROS.ROS_MASTER_URI = "http://10.0.3.5:11311";
ROS.Init(new string[0], "The_IMU_Tester_" + System.Environment.MachineName.Replace("-", "__"));
nh = new NodeHandle();
}).Start();
}
public void startListening(NodeHandle node)
{
tilt_min = -600;
tilt_max = 600;
pan_min = 0;
pan_max = 5100;
sub = node.subscribe<am.ArmStatus>("/arm/status", 1000, callbackMonitor);
//pub = node.advertise<am.ArmMovement>("/arm/movement", 1000);
}
public RosoutDebug()
{
InitializeComponent();
Loaded += (sender, args) =>
{
if (System.Diagnostics.Process.GetCurrentProcess().ProcessName == "devenv")
return;
ROS.WaitForMaster();
nh = new NodeHandle();
Init();
};
}
private void SubscribeToImage(string topic)
{
if (imagehandle == null)
imagehandle = new NodeHandle();
if (imgSub != null && imgSub.topic != topic)
{
imgSub.shutdown();
imgSub = null;
}
if (imgSub != null)
return;
Console.WriteLine("Subscribing to image at:= " + topic);
imgSub = imagehandle.subscribe<sm.CompressedImage>(topic, 1, updateImage);
}
public SimTime()
{
new Thread(() =>
{
while (!ROS.isStarted() && !ROS.shutting_down)
{
Thread.Sleep(100);
}
nh = new NodeHandle();
if (!ROS.shutting_down)
{
simTimeSubscriber = nh.subscribe<Clock>("/clock", 1, SimTimeCallback);
}
}).Start();
}
public static void start()
{
ROS.Init(new string[0], "rosout");
nh = new NodeHandle();
sub = nh.subscribe<Messages.rosgraph_msgs.Log>("/rosout", 10, rosoutCallback);
pub = nh.advertise<Messages.rosgraph_msgs.Log>("/rosout_agg", 1000, true);
new Thread(() =>
{
while (!ROS.ok)
{
Thread.Sleep(10);
}
}).Start();
}
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 = new NodeHandle();
Load();
}
}) { IsBackground = true }.Start();
}
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 static void Main(string[] args)
{
ROS.Init(args, "Talker");
NodeHandle node = new NodeHandle();
Publisher<m.String> Talker = node.advertise<m.String>("/chatter", 1);
int count = 0;
while (ROS.ok)
{
ROS.Info("Publishing a chatter message: \"Blah blah blah " + count + "\"");
String pow = new String("Blah blah blah " + (count++));
Talker.publish(pow);
Thread.Sleep(1000);
}
ROS.shutdown();
ROS.waitForShutdown();
}
public WindowScraper(string topic, Window w, TimeSpan period)
{
w.Dispatcher.Invoke(new Action(() =>
{
w.LocationChanged += (s, a) =>
{
window_left = (int) w.Left;
window_top = (int) w.Top;
};
}));
hwnd = new WindowInteropHelper(w).Handle;
period_ms = (int) Math.Floor(period.TotalMilliseconds);
timer = new Timer(callback, null, Timeout.Infinite, period_ms);
nh = new NodeHandle();
if (!topic.EndsWith("/compressed"))
{
Console.WriteLine("APPENDING /compressed TO TOPIC NAME TO MAKE IMAGE TRANSPORT HAPPY");
topic += "/compressed";
}
pub = nh.advertise<CompressedImage>(topic, 10);
}
// Use this for initialization
void Start()
{
print("Starting...");
TfTreeManager.Instance.AddListener(vis =>
{
//Debug.LogWarning("LaserMeshView has a tfvisualizer now!");
tfvisualizer = vis;
});
if (is_static)
{
theCloud = new mesh_pcloud[1];
GameObject theCloudObject = new GameObject("pointCloudContainer");
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[0] = theCloudObject.GetComponent <mesh_pcloud>();
registerMeshPclouds();
print("Starting to load static cloud from " + Mesh_File.name);
string[] pointLocations = Mesh_File.text.Split('\n');
string Version = pointLocations[1].Split(' ')[1];
string[] Fields = pointLocations[2].Split(' ');
string[] Sizes = pointLocations[3].Split(' ');
string[] Type = pointLocations[4].Split(' ');
string[] Count = pointLocations[5].Split(' ');
int Width = Int32.Parse(pointLocations[6].Split(' ')[1]);
int Height = Int32.Parse(pointLocations[7].Split(' ')[1]);
string[] Viewpoint = pointLocations[8].Split(' ');
int Points = Int32.Parse(pointLocations[9].Split(' ')[1]);
string Data = pointLocations[10].Split(' ')[1];
if (Data != "ascii")
{
print("Only ascii pcd format is supported...");
print(Data);
return;
}
//theCloud.vertexBuffers[theCloud.whichBuffer] = new Vector3[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.colorBuffers[theCloud.whichBuffer] = new Color[10 * vertexCount[pointShape]];//[(pointLocations.Length / 6) * vertexCount[pointShape]];
//theCloud.recalculateTriangles(10, indices[pointShape], vertexCount[pointShape]);
theCloud[0].vertexBuffers[theCloud[0].whichBuffer] = new Vector3[Points * vertexCount[pointShape]];
theCloud[0].colorBuffers[theCloud[0].whichBuffer] = new Color[Points * vertexCount[pointShape]];
theCloud[0].recalculateTriangles(Points, indices[pointShape], vertexCount[pointShape]);
for (int i = 11; i < Points + 11; i++)
{
try
{
string[] thisPoint = pointLocations[i].Split(' ');
float x = float.Parse(thisPoint[0]);
float y = float.Parse(thisPoint[1]);
float z = float.Parse(thisPoint[2]);
Color c;
if (thisPoint[3][0] != 'n')
{
byte[] color = System.BitConverter.GetBytes(float.Parse(thisPoint[3]));
c = new Color(color[2] * 0.00392156862F, color[1] * 0.00392156862F, color[0] * 0.00392156862F, 1);
}
else
{
c = new Color(0, 0, 0, 1);
}
addPoint(x, y, z, c);
}
catch (Exception e)
{
print(e);
print(pointLocations[i]);
}
}
print("number of points loaded: " + pointNum);
theCloud[0].whichBuffer = (theCloud[0].whichBuffer + 1) % 2;
theCloud[0].updateVertexData = true;
theCloud[0].updateColorData = true;
theCloud[0].updateTriangleData = true;
}
else
{
ROS.ROS_HOSTNAME = "asgard";
ROS.ROS_MASTER_URI = "http://thor:11311";
ROS.Init(new String[0], "mesh_pcloud_renderer");
nh = new NodeHandle();
//nh= rosmaster.getNodeHandle();
print("Subscribing");
theCloud = new mesh_pcloud[accumulate_data && max_acc_frames > 0 ? max_acc_frames : 1];
for (int i = 0; i < theCloud.Length; i++)
{
GameObject theCloudObject = new GameObject("pointCloudContainer" + i);
theCloudObject.transform.parent = gameObject.transform;
theCloudObject.AddComponent <mesh_pcloud>();
theCloud[i] = theCloudObject.GetComponent <mesh_pcloud>();
}
sub = nh.subscribe <Messages.sensor_msgs.PointCloud2>(topic_name, 2, subCallback);
unity_tf_msg = new Messages.geometry_msgs.PolygonStamped();
unity_tf_msg.header = new Messages.std_msgs.Header();
unity_tf_msg.polygon = new Messages.geometry_msgs.Polygon();
unity_tf_msg.header.frame_id = (is_static ? Mesh_File.name : topic_name);
unity_tf_msg.polygon.points = new Messages.geometry_msgs.Point32[3];
unity_tf_pub = nh.advertise <Messages.geometry_msgs.PolygonStamped>("/unity_tf", 10);
ThreadStart pub_thread_start = new ThreadStart(UnityTFPublisher);
pub_thread = new Thread(pub_thread_start);
pub_thread.Start();
}
print("Started");
//theCloud.Initialize();
}
void OnEnable()
{
m_Set = new NodeSet();
m_Node = m_Set.Create <MyNode>();
}
public NodePin Lock(NodePin parent, NodeHandle child)
{
return(Lock(parent, parent.LockType, child));
}
/// <summary>
/// Deletes history events.
/// </summary>
protected override void HistoryDeleteEvents(
ServerSystemContext context,
IList <DeleteEventDetails> nodesToUpdate,
IList <HistoryUpdateResult> results,
IList <ServiceResult> errors,
List <NodeHandle> nodesToProcess,
IDictionary <NodeId, NodeState> cache)
{
for (int ii = 0; ii < nodesToProcess.Count; ii++)
{
NodeHandle handle = nodesToProcess[ii];
DeleteEventDetails nodeToUpdate = nodesToUpdate[handle.Index];
HistoryUpdateResult result = results[handle.Index];
// delete events.
bool failed = false;
for (int jj = 0; jj < nodeToUpdate.EventIds.Count; jj++)
{
try
{
string eventId = new Guid(nodeToUpdate.EventIds[jj]).ToString();
if (!m_generator.DeleteEvent(eventId))
{
result.OperationResults.Add(StatusCodes.BadEventIdUnknown);
failed = true;
continue;
}
result.OperationResults.Add(StatusCodes.Good);
}
catch
{
result.OperationResults.Add(StatusCodes.BadEventIdUnknown);
failed = true;
}
}
// check if diagnostics are required.
if (failed)
{
if ((context.DiagnosticsMask & DiagnosticsMasks.OperationAll) != 0)
{
for (int jj = 0; jj < nodeToUpdate.EventIds.Count; jj++)
{
if (StatusCode.IsBad(result.OperationResults[jj]))
{
result.DiagnosticInfos.Add(ServerUtils.CreateDiagnosticInfo(Server, context.OperationContext, result.OperationResults[jj]));
}
}
}
}
// clear operation results if all good.
else
{
result.OperationResults.Clear();
}
// all done.
errors[handle.Index] = ServiceResult.Good;
}
}
bool configure(NodeHandle param)
{
// get model parameters
if (!param.getParam("k_m", ref propulsion_model_.parameters_.k_m))
{
return(false);
}
if (!param.getParam("k_t", ref propulsion_model_.parameters_.k_t))
{
return(false);
}
if (!param.getParam("CT0s", ref propulsion_model_.parameters_.CT0s))
{
return(false);
}
if (!param.getParam("CT1s", ref propulsion_model_.parameters_.CT1s))
{
return(false);
}
if (!param.getParam("CT2s", ref propulsion_model_.parameters_.CT2s))
{
return(false);
}
if (!param.getParam("J_M", ref propulsion_model_.parameters_.J_M))
{
return(false);
}
if (!param.getParam("l_m", ref propulsion_model_.parameters_.l_m))
{
return(false);
}
if (!param.getParam("Psi", ref propulsion_model_.parameters_.Psi))
{
return(false);
}
if (!param.getParam("R_A", ref propulsion_model_.parameters_.R_A))
{
return(false);
}
if (!param.getParam("alpha_m", ref propulsion_model_.parameters_.alpha_m))
{
return(false);
}
if (!param.getParam("beta_m", ref propulsion_model_.parameters_.beta_m))
{
return(false);
}
// #ifndef NDEBUG
// std::cout << "Loaded the following quadrotor propulsion model parameters from namespace " << param.getNamespace() << ":" << std::endl;
// std::cout << "k_m = " << propulsion_model_.parameters_.k_m << std::endl;
// std::cout << "k_t = " << propulsion_model_.parameters_.k_t << std::endl;
// std::cout << "CT2s = " << propulsion_model_.parameters_.CT2s << std::endl;
// std::cout << "CT1s = " << propulsion_model_.parameters_.CT1s << std::endl;
// std::cout << "CT0s = " << propulsion_model_.parameters_.CT0s << std::endl;
// std::cout << "Psi = " << propulsion_model_.parameters_.Psi << std::endl;
// std::cout << "J_M = " << propulsion_model_.parameters_.J_M << std::endl;
// std::cout << "R_A = " << propulsion_model_.parameters_.R_A << std::endl;
// std::cout << "l_m = " << propulsion_model_.parameters_.l_m << std::endl;
// std::cout << "alpha_m = " << propulsion_model_.parameters_.alpha_m << std::endl;
// std::cout << "beta_m = " << propulsion_model_.parameters_.beta_m << std::endl;
// #endif
initial_voltage_ = 14.8;
param.getParam("supply_voltage", ref initial_voltage_);
reset();
return(true);
}
/// <summary>
/// Returns a unique handle for the node.
/// </summary>
protected override NodeHandle GetManagerHandle(ServerSystemContext context, NodeId nodeId, IDictionary <NodeId, NodeState> cache)
{
lock (Lock)
{
// quickly exclude nodes that are not in the namespace.
if (!IsNodeIdInNamespace(nodeId))
{
return(null);
}
// check cache.
if (cache != null)
{
NodeState node = null;
if (cache.TryGetValue(nodeId, out node))
{
return(new NodeHandle(nodeId, node));
}
}
NodeHandle handle = null;
try
{
// check if node already being monitored.
if (MonitoredNodes != null)
{
MonitoredNode2 monitoredNode2 = null;
if (MonitoredNodes.TryGetValue(nodeId, out monitoredNode2))
{
handle = new NodeHandle(nodeId, monitoredNode2.Node);
handle.MonitoredNode = monitoredNode2;
return(handle);
}
}
// check for predefined nodes.
if (PredefinedNodes != null)
{
NodeState node = null;
if (PredefinedNodes.TryGetValue(nodeId, out node))
{
return(handle = new NodeHandle(nodeId, node));
}
}
// parse the identifier.
AeParsedNodeId parsedNodeId = AeParsedNodeId.Parse(nodeId);
if (parsedNodeId != null)
{
if (parsedNodeId.RootType == AeModelUtils.AeEventTypeMapping && m_typeCache != null)
{
AeEventTypeMappingState mappingNode = m_typeCache.GetMappingNode(SystemContext, nodeId);
if (mappingNode != null)
{
return(handle = new NodeHandle(nodeId, mappingNode));
}
return(null);
}
handle = new NodeHandle();
handle.NodeId = nodeId;
handle.Validated = false;
handle.Node = null;
handle.ParsedNodeId = parsedNodeId;
return(handle);
}
}
finally
{
if (handle != null && handle.Node != null && cache != null)
{
cache.Add(nodeId, handle.Node);
}
}
return(null);
}
}
InputPortID ITaskPort <IOffsettable> .GetPort(NodeHandle handle) => (InputPortID)SimulationPorts.TimeOffset;
InputPortID ITaskPort <IWeightable> .GetPort(NodeHandle handle) => (InputPortID)SimulationPorts.WeightPort;
public static void Connect <TType, TBuffer>(this NodeSet set, NodeHandle <ComponentNode> from, NodeHandle <TType> to, DataInput <TType, Buffer <TBuffer> > port, NodeSet.ConnectionType type = NodeSet.ConnectionType.Normal)
where TType : NodeDefinition
where TBuffer : struct, IBufferElementData
{
set.Connect(from, ComponentNode.Output <TBuffer>(), to, port, type);
}
public static void Connect <TType, TData>(this NodeSet set, NodeHandle <TType> from, DataOutput <TType, TData> port, NodeHandle <ComponentNode> to, NodeSet.ConnectionType type = NodeSet.ConnectionType.Normal)
where TType : NodeDefinition
where TData : struct, IComponentData
{
set.Connect(from, port, to, ComponentNode.Input <TData>(), type);
}
/// <summary>
/// Called when the scan attributes timer expires.
/// </summary>
private void DoScan(Dictionary <string, HdaSubscribeAttributeRequest> requests, bool initialUpdate)
{
try
{
ComHdaClientManager system = (ComHdaClientManager)m_context.SystemHandle;
ComHdaClient client = (ComHdaClient)system.SelectClient(m_context, false);
// ensure all requests have valid handles.
AssignHandles(client, requests);
// collect the list of attributes that need reading.
HdaReadRequestCollection itemsToRead = new HdaReadRequestCollection();
List <HdaSubscribeAttributeRequest> subscribeRequests = new List <HdaSubscribeAttributeRequest>();
List <HdaReadRequest> readRequestsForSubscribeRequests = new List <HdaReadRequest>();
lock (m_lock)
{
DateTime now = DateTime.UtcNow;
foreach (KeyValuePair <string, HdaSubscribeAttributeRequest> entry in requests)
{
// check if it is time for an update.
if (!initialUpdate && entry.Value.NextUpdateTime > now)
{
continue;
}
// create a read request for each monitored item.
bool somethingToDo = false;
for (int ii = 0; ii < entry.Value.MonitoredItems.Count; ii++)
{
MonitoredItem monitoredItem = entry.Value.MonitoredItems[ii];
NodeHandle handle = monitoredItem.ManagerHandle as NodeHandle;
if (handle == null)
{
continue;
}
// check if item is not valid.
if (entry.Value.ServerHandle == null)
{
monitoredItem.QueueValue(null, StatusCodes.BadNodeIdUnknown);
continue;
}
ReadValueId valueToRead = monitoredItem.GetReadValueId();
bool queued = false;
StatusCode error = itemsToRead.Add(handle.Node, valueToRead, out queued);
if (StatusCode.IsBad(error))
{
monitoredItem.QueueValue(null, error);
continue;
}
if (!somethingToDo && queued)
{
// add server handle to read request.
HdaReadRequest request = (HdaReadRequest)valueToRead.Handle;
request.ServerHandle = entry.Value.ServerHandle.Value;
// save mapping between subscribe request and read requests.
subscribeRequests.Add(entry.Value);
readRequestsForSubscribeRequests.Add(request);
somethingToDo = true;
}
}
}
// check if nothing to do.
if (requests.Count == 0)
{
return;
}
}
// read the attributes from the server.
client.Read(itemsToRead, true);
// update the monitored items.
lock (m_lock)
{
for (int ii = 0; ii < subscribeRequests.Count; ii++)
{
subscribeRequests[ii].QueueValues(m_context, readRequestsForSubscribeRequests[ii], initialUpdate);
}
}
}
catch (Exception e)
{
Utils.Trace(e, "Unexpected error scanning attributes in HDA COM server.");
}
}
/// <summary>
/// Writes the value for the specified attributes.
/// </summary>
public override void Write(
OperationContext context,
IList <WriteValue> nodesToWrite,
IList <ServiceResult> errors)
{
ServerSystemContext systemContext = SystemContext.Copy(context);
IDictionary <NodeId, NodeState> operationCache = new NodeIdDictionary <NodeState>();
List <DataSourceClient.WriteRequest> writeRequests = new List <DataSourceClient.WriteRequest>();
lock (Lock)
{
for (int ii = 0; ii < nodesToWrite.Count; ii++)
{
WriteValue nodeToWrite = nodesToWrite[ii];
// skip items that have already been processed.
if (nodeToWrite.Processed)
{
continue;
}
// check for valid handle.
NodeHandle handle = GetManagerHandle(systemContext, nodeToWrite.NodeId, operationCache);
if (handle == null)
{
continue;
}
// owned by this node manager.
nodeToWrite.Processed = true;
// index range is not supported.
if (nodeToWrite.AttributeId != Attributes.Value)
{
if (!String.IsNullOrEmpty(nodeToWrite.IndexRange))
{
errors[ii] = StatusCodes.BadWriteNotSupported;
continue;
}
}
// check if the node is a area in memory.
if (handle.Node == null)
{
errors[ii] = StatusCodes.BadNodeIdUnknown;
continue;
}
// check if the request if for a remote node.
RemoteNode remoteNode = null;
if (m_remoteNodes.TryGetValue(handle.NodeId, out remoteNode))
{
// check for theorectical access.
BaseVariableState variable = handle.Node as BaseVariableState;
if (variable == null || nodeToWrite.AttributeId != Attributes.Value || (variable.AccessLevel & AccessLevels.CurrentWrite) == 0)
{
errors[ii] = StatusCodes.BadNotWritable;
continue;
}
// check for access based on current user credentials.
if (!CheckWriteAccess(systemContext, remoteNode))
{
errors[ii] = StatusCodes.BadUserAccessDenied;
continue;
}
DataSourceClient.WriteRequest request = new DataSourceClient.WriteRequest();
request.RemoteId = remoteNode.RemoteId;
request.WriteValue = nodeToWrite;
request.Index = ii;
writeRequests.Add(request);
errors[ii] = StatusCodes.BadNoCommunication;
continue;
}
// write the attribute value.
errors[ii] = handle.Node.WriteAttribute(
systemContext,
nodeToWrite.AttributeId,
nodeToWrite.ParsedIndexRange,
nodeToWrite.Value);
// updates to source finished - report changes to monitored items.
handle.Node.ClearChangeMasks(systemContext, false);
}
// check for nothing to do.
if (writeRequests.Count == 0)
{
return;
}
}
// update the remote data source.
List <ServiceResult> results = m_source.Write(writeRequests);
for (int ii = 0; ii < writeRequests.Count; ii++)
{
errors[writeRequests[ii].Index] = results[ii];
}
}
static void Main(string[] args)
{
ROS.Init(args, "tf_example");
var asyncSpinner = new AsyncSpinner();
asyncSpinner.Start();
NodeHandle nh = new NodeHandle();
ROS.Info()("This node will create a Transformer to compare lookup results between four source/target " +
"frame pairs of an OpenNI2 node's transform tree with ones observed in linux with tf_echo"
);
while (ROS.OK && !ROS.ShuttingDown && (!Master.GetNodes().Result.Contains("/camera/driver")))
{
ROS.Error()("For this to work, you need to \"roslaunch openni2_launch openni2.launch\" on a PC with" +
"an ASUS Xtion or PrimeSense Carmine sensor plugged into it, and connect to the same master"
);
Thread.Sleep(2000);
}
if (ROS.OK && !ROS.ShuttingDown)
{
tfer = new tf.Transformer(false);
#region tf_echo results
/*
* tf_echo camera_link camera_rgb_frame
* (0.0,-0.045,0.0)
* (0,0,0,1)
*/
tf.Transform result1 = new tf.Transform()
{
basis = new tf.Quaternion(0, 0, 0, 1),
origin = new tf.Vector3(0, -0.045, 0),
child_frame_id = "camera_rgb_frame",
frame_id = "camera_link"
};
/*
* tf_echo camera_link camera_rgb_optical_frame
* (0.0,-0.045,0.0)
* (-0.5,0.5,-0.5,0.5)
*/
tf.Transform result2 = new tf.Transform()
{
basis = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
origin = new tf.Vector3(0.0, 0.0, 0.0),
child_frame_id = "camera_rgb_optical_frame",
frame_id = "camera_rgb_frame"
};
/*
* tf_echo camera_rgb_frame camera_depth_frame
* (0.0,0.25,0.0)
* (0,0,0,1)
*/
tf.Transform result3 = new tf.Transform()
{
basis = new tf.Quaternion(0, 0, 0, 1),
origin = new tf.Vector3(0.0, -0.02, 0.0),
child_frame_id = "camera_depth_frame",
frame_id = "camera_link"
};
/*
* tf_echo camera_rgb_optical_frame camera_depth_frame
* (-0.25,0.0,0.0)
* (0.5,-0.5,0.5,0.5)
*/
tf.Transform result4 = new tf.Transform()
{
basis = new tf.Quaternion(-0.5, 0.5, -0.5, 0.5),
origin = new tf.Vector3(0.0, 0.0, 0.0),
child_frame_id = "camera_depth_optical_frame",
frame_id = "camera_depth_frame"
};
#endregion
tf.Transform test1 = null, test2 = null, test3 = null, test4 = null;
do
{
if (test1 == null || !string.Equals(result1.ToString(), test1.ToString()))
{
test1 = testLookup(result1);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test2 == null || !string.Equals(result2.ToString(), test2.ToString()))
{
test2 = testLookup(result2);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test3 == null || !string.Equals(result3.ToString(), test3.ToString()))
{
test3 = testLookup(result3);
}
if (!ROS.OK || ROS.ShuttingDown)
{
break;
}
if (test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
{
test4 = testLookup(result4);
}
Thread.Sleep(100);
} while (ROS.OK && !ROS.ShuttingDown && (
test1 == null || !string.Equals(result1.ToString(), test1.ToString()) ||
test2 == null || !string.Equals(result2.ToString(), test2.ToString()) ||
test3 == null || !string.Equals(result3.ToString(), test3.ToString()) ||
test4 == null || !string.Equals(result4.ToString(), test4.ToString()))
);
}
if (ROS.OK && !ROS.ShuttingDown)
{
Console.WriteLine("\n\n\nALL TFs MATCH!\n\nPress enter to quit");
Console.ReadLine();
ROS.Shutdown();
}
}
/*static void Main(string[] args)
* {
* Console.WriteLine("Start ROS");
* ROS.Init(new string[0], "ActionClient");
* NodeHandle clientNodeHandle = new NodeHandle();
*
* Console.WriteLine("Create client");
* var actionClient = new ActionClient<SingleJointPositionGoal, SingleJointPositionResult,
* SingleJointPositionFeedback>("ActionTest", clientNodeHandle);
*
* Console.WriteLine("Wait for client and server to negotiate connection");
* bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 10));
*
*
* if (started || true)
* {
* var goal = new SingleJointPositionGoal();
* goal.position = 12.0;
* goal.max_velocity = 42.0;
*
* Console.WriteLine("Send goal from client");
* var transition = false;
* actionClient.SendGoal(goal,
* (goalHandle) => { Console.WriteLine($"Transition: {goalHandle}"); transition = true; },
* (goalHandle, feedback) => { Console.WriteLine($"Feedback: {feedback}"); });
*
*
* Console.WriteLine("Wait for action client to receive transition");
* while (!transition)
* {
* Thread.Sleep(1);
* }
* } else
* {
* Console.WriteLine("Negotiation with server failed!");
* }
* }*/
static void Main(string[] args)
{
Console.WriteLine("Start ROS");
ROS.Init(new string[0], "ActionClient");
var asyncSpinner = new AsyncSpinner();
asyncSpinner.Start();
NodeHandle clientNodeHandle = ROS.GlobalNodeHandle;
Console.WriteLine("Create client");
var actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback>("test_action", clientNodeHandle);
Console.WriteLine("Wait for client and server to negotiate connection");
bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 3));
if (started)
{
int counter = 0;
var dict = new Dictionary <int, Messages.actionlib.TestGoal>();
var semaphore = new Semaphore(10, 10);
while (!Console.KeyAvailable)
{
var now = DateTime.UtcNow;
semaphore.WaitOne();
Console.WriteLine($"Waited: {DateTime.UtcNow - now}");
var goal = new Messages.actionlib.TestGoal();
goal.goal = counter;
dict[counter] = goal;
counter += 1;
Console.WriteLine($"Send goal {goal.goal} from client");
var cts = new CancellationTokenSource();
actionClient.SendGoalAsync(goal,
(goalHandle) =>
{
if (goalHandle.State == CommunicationState.DONE)
{
semaphore.Release();
int g = goalHandle.Goal.Goal.goal;
var result = goalHandle.Result;
if (result != null)
{
Console.WriteLine($"Got Result for goal {g}: {goalHandle.Result.result}");
}
else
{
Console.WriteLine($"Result for goal {g} is NULL!");
}
dict.Remove(g);
}
},
(goalHandle, feedback) =>
{
Console.WriteLine($"Feedback: {feedback}");
},
cts.Token
).GetAwaiter().GetResult();
}
Console.WriteLine("Wait for 15s for open goals");
var timeOut = new TimeSpan(0, 0, 15);
var start = DateTime.UtcNow;
while ((DateTime.UtcNow - start <= timeOut) && (dict.Count > 0))
{
Thread.Sleep(1);
}
if (dict.Count == 0)
{
Console.WriteLine("All goals have been reached!");
}
else
{
Console.WriteLine("TIMEOUT: There are still open goals");
}
}
else
{
Console.WriteLine("Negotiation with server failed!");
}
Console.WriteLine("Shutdown ROS");
actionClient.Shutdown();
clientNodeHandle.Shutdown();
ROS.Shutdown();
}
/// <summary>
/// Verifies that the specified node exists.
/// </summary>
protected override NodeState ValidateNode(
ServerSystemContext context,
NodeHandle handle,
IDictionary <NodeId, NodeState> cache)
{
// not valid if no root.
if (handle == null)
{
return(null);
}
// check if previously validated.
if (handle.Validated)
{
return(handle.Node);
}
NodeState target = null;
// check if already in the cache.
if (cache != null)
{
if (cache.TryGetValue(handle.NodeId, out target))
{
// nulls mean a NodeId which was previously found to be invalid has been referenced again.
if (target == null)
{
return(null);
}
handle.Node = target;
handle.Validated = true;
return(handle.Node);
}
target = null;
}
try
{
// check if the node id has been parsed.
AeParsedNodeId parsedNodeId = handle.ParsedNodeId as AeParsedNodeId;
if (parsedNodeId == null)
{
return(null);
}
ComAeClient client = m_system.SelectClient(context, false);
switch (parsedNodeId.RootType)
{
case AeModelUtils.AeSimpleEventType:
case AeModelUtils.AeTrackingEventType:
case AeModelUtils.AeConditionEventType:
{
if (m_typeCache == null)
{
return(null);
}
BaseObjectTypeState eventTypeNode = null;
NodeId rootId = AeParsedNodeId.Construct(parsedNodeId.RootType, parsedNodeId.CategoryId, parsedNodeId.ConditionName, parsedNodeId.NamespaceIndex);
if (!m_typeCache.EventTypeNodes.TryGetValue(rootId, out eventTypeNode))
{
return(null);
}
target = eventTypeNode;
break;
}
case AeModelUtils.AeArea:
{
ComAeBrowserClient browser = new ComAeBrowserClient(client, null);
target = browser.FindArea(context, parsedNodeId.RootId, NamespaceIndex);
browser.Dispose();
handle.Validated = true;
handle.Node = target;
return(handle.Node);
}
case AeModelUtils.AeSource:
{
ComAeBrowserClient browser = new ComAeBrowserClient(client, null);
target = browser.FindSource(context, parsedNodeId.RootId, parsedNodeId.ComponentPath, NamespaceIndex);
browser.Dispose();
handle.Validated = true;
handle.Node = target;
return(handle.Node);
}
case AeModelUtils.AeCondition:
{
target = new AeConditionState(context, handle, m_templateAlarm.Acknowledge);
break;
}
}
// node does not exist.
if (target == null)
{
return(null);
}
if (!String.IsNullOrEmpty(parsedNodeId.ComponentPath))
{
// validate component.
NodeState component = target.FindChildBySymbolicName(context, parsedNodeId.ComponentPath);
// component does not exist.
if (component == null)
{
return(null);
}
target = component;
}
// found a valid component.
handle.Validated = true;
handle.Node = target;
return(handle.Node);
}
finally
{
// store the node in the cache to optimize subsequent lookups.
if (cache != null)
{
cache.Add(handle.NodeId, target);
}
}
}
private void CreateFromHandle()
{
fromHandle = new NodeHandle(fromNode, NodeHandle.HandleOrientation.RIGHT);
}
/// <summary>
/// Reads history events.
/// </summary>
protected override void HistoryReadEvents(
ServerSystemContext context,
ReadEventDetails details,
TimestampsToReturn timestampsToReturn,
IList <HistoryReadValueId> nodesToRead,
IList <HistoryReadResult> results,
IList <ServiceResult> errors,
List <NodeHandle> nodesToProcess,
IDictionary <NodeId, NodeState> cache)
{
for (int ii = 0; ii < nodesToProcess.Count; ii++)
{
NodeHandle handle = nodesToProcess[ii];
HistoryReadValueId nodeToRead = nodesToRead[handle.Index];
HistoryReadResult result = results[handle.Index];
HistoryReadRequest request = null;
// load an exising request.
if (nodeToRead.ContinuationPoint != null)
{
request = LoadContinuationPoint(context, nodeToRead.ContinuationPoint);
if (request == null)
{
errors[handle.Index] = StatusCodes.BadContinuationPointInvalid;
continue;
}
}
// create a new request.
else
{
request = CreateHistoryReadRequest(
context,
details,
handle,
nodeToRead);
}
// process events until the max is reached.
HistoryEvent events = new HistoryEvent();
while (request.NumValuesPerNode == 0 || events.Events.Count < request.NumValuesPerNode)
{
if (request.Events.Count == 0)
{
break;
}
BaseEventState e = null;
if (request.TimeFlowsBackward)
{
e = request.Events.Last.Value;
request.Events.RemoveLast();
}
else
{
e = request.Events.First.Value;
request.Events.RemoveFirst();
}
events.Events.Add(GetEventFields(request, e));
}
errors[handle.Index] = ServiceResult.Good;
// check if a continuation point is requred.
if (request.Events.Count > 0)
{
// only set if both end time and start time are specified.
if (details.StartTime != DateTime.MinValue && details.EndTime != DateTime.MinValue)
{
result.ContinuationPoint = SaveContinuationPoint(context, request);
}
}
// check if no data returned.
else
{
errors[handle.Index] = StatusCodes.GoodNoData;
}
// return the data.
result.HistoryData = new ExtensionObject(events);
}
}
public void startListening(NodeHandle node)
{
sub = node.subscribe <m.Float32>("/Voltage", 1000, callbackVoltMonitor);
}
/// <summary>
/// Creates a new history request.
/// </summary>
private HistoryReadRequest CreateHistoryReadRequest(
ServerSystemContext context,
ReadEventDetails details,
NodeHandle handle,
HistoryReadValueId nodeToRead)
{
FilterContext filterContext = new FilterContext(context.NamespaceUris, context.TypeTable, context.PreferredLocales);
LinkedList <BaseEventState> events = new LinkedList <BaseEventState>();
for (ReportType ii = ReportType.FluidLevelTest; ii <= ReportType.InjectionTest; ii++)
{
DataView view = null;
if (handle.Node is WellState)
{
view = m_generator.ReadHistoryForWellId(
ii,
(string)handle.Node.NodeId.Identifier,
details.StartTime,
details.EndTime);
}
else
{
view = m_generator.ReadHistoryForArea(
ii,
handle.Node.NodeId.Identifier as string,
details.StartTime,
details.EndTime);
}
LinkedListNode <BaseEventState> pos = events.First;
bool sizeLimited = (details.StartTime == DateTime.MinValue || details.EndTime == DateTime.MinValue);
foreach (DataRowView row in view)
{
// check if reached max results.
if (sizeLimited)
{
if (events.Count >= details.NumValuesPerNode)
{
break;
}
}
BaseEventState e = m_generator.GetReport(context, NamespaceIndex, ii, row.Row);
if (details.Filter.WhereClause != null && details.Filter.WhereClause.Elements.Count > 0)
{
if (!details.Filter.WhereClause.Evaluate(filterContext, e))
{
continue;
}
}
bool inserted = false;
for (LinkedListNode <BaseEventState> jj = pos; jj != null; jj = jj.Next)
{
if (jj.Value.Time.Value > e.Time.Value)
{
events.AddBefore(jj, e);
pos = jj;
inserted = true;
break;
}
}
if (!inserted)
{
events.AddLast(e);
pos = null;
}
}
}
HistoryReadRequest request = new HistoryReadRequest();
request.Events = events;
request.TimeFlowsBackward = details.StartTime == DateTime.MinValue || (details.EndTime != DateTime.MinValue && details.EndTime < details.StartTime);
request.NumValuesPerNode = details.NumValuesPerNode;
request.Filter = details.Filter;
request.FilterContext = filterContext;
return(request);
}
protected abstract NodePin Lock(NodePin parent, LockType ltype, NodeHandle child);
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
poseTypeSrv = nh.advertiseService <SetInt.Request, SetInt.Response> ("/quad_rotor/camera_pose_type", SetCameraPoseType);
distanceSrv = nh.advertiseService <SetFloat.Request, SetFloat.Response> ("/quad_rotor/camera_distance", SetFollowDistance);
}
/// <summary>
/// Creates a instance of <c>RosRoboChatActionClient</c>
/// </summary>
/// <param name="nodeHandle">A node handle</param>
public RosRoboChatActionClient(NodeHandle nodeHandle)
{
channelCommandService = nodeHandle.ServiceClient <rosgardener.SetChannelCommand>(ROBOCHAT_CHANNEL_SERVICE_NAME);
messageCommandService = nodeHandle.ServiceClient <rosgardener.SetMessageCommand>(ROBOCHAT_MESSAGE_SERVICE_NAME);
rosRoboChatActionClient = new ActionClient <rosgardener.RobochatQueryGoal, rosgardener.RobochatQueryResult, rosgardener.RobochatQueryFeedback>(ROBOCHAT_ACTION_NAME, nodeHandle);
}
/*static void Main(string[] args)
* {
* Console.WriteLine("Start ROS");
* ROS.Init(new string[0], "ActionClient");
* NodeHandle clientNodeHandle = new NodeHandle();
*
* Console.WriteLine("Create client");
* var actionClient = new ActionClient<SingleJointPositionGoal, SingleJointPositionResult,
* SingleJointPositionFeedback>("ActionTest", clientNodeHandle);
*
* Console.WriteLine("Wait for client and server to negotiate connection");
* bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 10));
*
*
* if (started || true)
* {
* var goal = new SingleJointPositionGoal();
* goal.position = 12.0;
* goal.max_velocity = 42.0;
*
* Console.WriteLine("Send goal from client");
* var transition = false;
* actionClient.SendGoal(goal,
* (goalHandle) => { Console.WriteLine($"Transition: {goalHandle}"); transition = true; },
* (goalHandle, feedback) => { Console.WriteLine($"Feedback: {feedback}"); });
*
*
* Console.WriteLine("Wait for action client to receive transition");
* while (!transition)
* {
* Thread.Sleep(1);
* }
* } else
* {
* Console.WriteLine("Negotiation with server failed!");
* }
* }*/
static void Main(string[] args)
{
Environment.SetEnvironmentVariable("ROS_HOSTNAME", "localhost");
Environment.SetEnvironmentVariable("ROS_IP", "127.0.0.1");
Environment.SetEnvironmentVariable("ROS_MASTER_URI", "http://localhost:11311/");
Console.WriteLine("Start ROS");
ROS.Init(ref args, "ActionClient");
ICallbackQueue callbackQueue = new CallbackQueue();
var asyncSpinner = new AsyncSpinner(callbackQueue);
asyncSpinner.Start();
NodeHandle clientNodeHandle = new NodeHandle(callbackQueue); //ROS.GlobalNodeHandle;
Console.WriteLine("Create client");
var actionClient = new ActionClient <Messages.actionlib.TestGoal, Messages.actionlib.TestResult,
Messages.actionlib.TestFeedback>("test_action", clientNodeHandle);
Console.WriteLine("Wait for client and server to negotiate connection");
bool started = actionClient.WaitForActionServerToStart(new TimeSpan(0, 0, 60));
if (started)
{
int counter = 0;
var dict = new Dictionary <int, Messages.actionlib.TestGoal>();
while (!Console.KeyAvailable)
{
var now = DateTime.UtcNow;
Console.WriteLine($"Waited: {DateTime.UtcNow - now}");
var goal = new Messages.actionlib.TestGoal
{
goal = counter
};
dict[counter] = goal;
counter += 1;
Console.WriteLine($"------------------> Send goal {goal.goal} from client");
var cts = new CancellationTokenSource();
var goalTask = actionClient.SendGoalAsync(goal,
(goalHandle) =>
{
if (goalHandle.State == CommunicationState.DONE)
{
int g = goalHandle.Goal.Goal.goal;
var resultt = goalHandle.Result;
if (resultt != null)
{
Console.WriteLine($"------------------> Got Result for goal {g}: {resultt.result}");
}
else
{
Console.WriteLine($"Result for goal {g} is NULL!");
}
dict.Remove(g);
}
},
(goalHandle, feedback) =>
{
Console.WriteLine($"------------------> Got Feedback: {feedback.Feedback.feedback}");
},
cts.Token
);
var waiter = goalTask.GetAwaiter();
goalTask.Wait();
Console.ReadLine();
while (!waiter.IsCompleted)
{
Thread.Yield();
}
var result = waiter.GetResult();
break;
}
Console.WriteLine("Wait for 15s for open goals");
var timeOut = new TimeSpan(0, 0, 15);
var start = DateTime.UtcNow;
while ((DateTime.UtcNow - start <= timeOut) && (dict.Count > 0))
{
Thread.Sleep(1);
}
if (dict.Count == 0)
{
Console.WriteLine("All goals have been reached!");
}
else
{
Console.WriteLine("TIMEOUT: There are still open goals");
}
}
else
{
Console.WriteLine("Negotiation with server failed!");
}
Console.WriteLine("Shutdown ROS");
actionClient.Shutdown();
clientNodeHandle.shutdown();
ROS.shutdown();
}
/// <summary>
/// Reads the value for the specified attribute.
/// </summary>
public override void Read(
OperationContext context,
double maxAge,
IList <ReadValueId> nodesToRead,
IList <DataValue> values,
IList <ServiceResult> errors)
{
ServerSystemContext systemContext = SystemContext.Copy(context);
IDictionary <NodeId, NodeState> operationCache = new NodeIdDictionary <NodeState>();
List <DataSourceClient.ReadRequest> readRequests = new List <DataSourceClient.ReadRequest>();
lock (Lock)
{
for (int ii = 0; ii < nodesToRead.Count; ii++)
{
ReadValueId nodeToRead = nodesToRead[ii];
// skip items that have already been processed.
if (nodeToRead.Processed)
{
continue;
}
// check for valid handle.
NodeHandle handle = GetManagerHandle(systemContext, nodeToRead.NodeId, operationCache);
if (handle == null)
{
continue;
}
// owned by this node manager.
nodeToRead.Processed = true;
// create an initial value.
DataValue value = values[ii] = new DataValue();
value.Value = null;
value.ServerTimestamp = DateTime.UtcNow;
value.SourceTimestamp = DateTime.MinValue;
value.StatusCode = StatusCodes.Good;
// check if the node is a area in memory.
if (handle.Node == null)
{
errors[ii] = StatusCodes.BadNodeIdUnknown;
continue;
}
if (nodeToRead.AttributeId == Attributes.Value)
{
// check if the request if for a remote node.
RemoteNode remoteNode = null;
if (m_remoteNodes.TryGetValue(handle.NodeId, out remoteNode))
{
DataSourceClient.ReadRequest request = new DataSourceClient.ReadRequest();
request.RemoteId = remoteNode.RemoteId;
request.ReadValueId = nodeToRead;
request.Value = value;
request.Index = ii;
readRequests.Add(request);
errors[ii] = StatusCodes.BadNoCommunication;
continue;
}
}
// read the attribute value.
errors[ii] = handle.Node.ReadAttribute(
systemContext,
nodeToRead.AttributeId,
nodeToRead.ParsedIndexRange,
nodeToRead.DataEncoding,
value);
}
// check for nothing to do.
if (readRequests.Count == 0)
{
return;
}
}
// read from the remote data source.
List <ServiceResult> results = m_source.Read(readRequests);
for (int ii = 0; ii < readRequests.Count; ii++)
{
values[readRequests[ii].Index] = readRequests[ii].Value;
errors[readRequests[ii].Index] = results[ii];
}
}
public abstract ILockStrategy CreateLock(NodeHandle handle, out object refobj);
public Map_server(NodeHandle nh)
{
this.rosNodeHandle = nh;
map_resp_ = new nm.GetMap.Response();
}
/// <summary>
/// Returns a unique handle for the node.
/// </summary>
protected virtual NodeHandle GetManagerHandle(ServerSystemContext context, NodeId nodeId, IDictionary<NodeId,NodeState> cache)
{
if (!IsNodeIdInNamespace(nodeId))
{
return null;
}
if (m_predefinedNodes != null)
{
NodeState node = null;
if (m_predefinedNodes.TryGetValue(nodeId, out node))
{
NodeHandle handle = new NodeHandle();
handle.NodeId = nodeId;
handle.Node = node;
handle.Validated = true;
return handle;
}
}
return null;
}
private void SetupNamespace(string Namespace)
{
if (Process.GetCurrentProcess().ProcessName == "devenv")
return;
if (nh == null)
nh = new NodeHandle();
if (dynamic != null && dynamic.Namespace != Namespace)
{
dynamic = null;
}
if (dynamic == null)
{
dynamic = new DynamicReconfigureInterface(nh, Namespace);
dynamic.SubscribeForUpdates();
dynamic.DescribeParameters(DescriptionRecieved);
}
}
private void CreateToHandle()
{
toHandle = new NodeHandle(toNode, NodeHandle.HandleOrientation.LEFT);
}
InputPortID ITaskPort <IPlayable> .GetPort(NodeHandle handle) => (InputPortID)SimulationPorts.PlayPort;
public void shutdown()
{
if (dynamic != null)
{
dynamic = null;
}
if (nh != null)
{
nh.shutdown();
nh = null;
}
}
/// <summary>
/// Rewrite the entire BTree as a transaction to include the provided items. This method is Thread safe.
/// If the input is already sorted, use BulkInsertOptions overload to specify InputIsSorted = true.
/// </summary>
public int BulkInsert(IEnumerable <KeyValuePair <TKey, TValue> > items, BulkInsertOptions bulkOptions)
{
NodePin oldRoot = null;
if (bulkOptions.InputIsSorted == false)
{
KeyValueSerializer <TKey, TValue> kvserializer = new KeyValueSerializer <TKey, TValue>(_options.KeySerializer, _options.ValueSerializer);
items = new OrderedKeyValuePairs <TKey, TValue>(_options.KeyComparer, items, kvserializer)
{
DuplicateHandling = bulkOptions.DuplicateHandling
};
}
List <IStorageHandle> handles = new List <IStorageHandle>();
try
{
int counter = 0;
using (RootLock root = LockRoot(LockType.Insert, "Merge", false))
{
if (root.Pin.Ptr.Count != 1)
{
throw new InvalidDataException();
}
NodeHandle oldRootHandle = root.Pin.Ptr[0].ChildNode;
oldRoot = _storage.Lock(root.Pin, oldRootHandle);
if (oldRoot.Ptr.Count == 0 || bulkOptions.ReplaceContents)
{
// Currently empty, so just enforce duplicate keys...
items = OrderedKeyValuePairs <TKey, TValue>
.WithDuplicateHandling(items,
new KeyValueComparer <TKey, TValue>(_options.KeyComparer),
bulkOptions.DuplicateHandling);
}
else
{
// Merging with existing data and enforce duplicate keys...
items = OrderedKeyValuePairs <TKey, TValue>
.Merge(_options.KeyComparer, bulkOptions.DuplicateHandling, EnumerateNodeContents(oldRoot), items);
}
Node newtree = BulkWrite(handles, ref counter, items);
if (newtree == null) // null when enumeration was empty
{
return(0);
}
using (NodeTransaction trans = _storage.BeginTransaction())
{
Node rootNode = trans.BeginUpdate(root.Pin);
rootNode.ReplaceChild(0, oldRootHandle, new NodeHandle(newtree.StorageHandle));
trans.Commit();
}
_count = counter;
}
//point of no return...
handles.Clear();
DeleteTree(oldRoot);
oldRoot = null;
if (bulkOptions.CommitOnCompletion)
{
//Since transaction logs do not deal with bulk-insert, we need to commit our current state
Commit();
}
return(counter);
}
catch
{
if (oldRoot != null)
{
oldRoot.Dispose();
}
foreach (IStorageHandle sh in handles)
{
try { _storage.Storage.Destroy(sh); }
catch (ThreadAbortException) { throw; }
catch { continue; }
}
throw;
}
}
InputPortID ITaskPort <ISeekable> .GetPort(NodeHandle handle) => (InputPortID)SimulationPorts.SeekPort;
private Node BulkWrite(ICollection <IStorageHandle> handles, ref int counter, IEnumerable <KeyValuePair <TKey, TValue> > itemsEnum)
{
List <Node> working = new List <Node>();
Node leafNode = null;
using (IEnumerator <KeyValuePair <TKey, TValue> > items = itemsEnum.GetEnumerator())
{
bool more = items.MoveNext();
while (more)
{
NodeHandle handle = new NodeHandle(_storage.Storage.Create());
handles.Add(handle.StoreHandle);
leafNode = new Node(handle.StoreHandle, _options.MaximumValueNodes);
while (leafNode.Count < leafNode.Size && more)
{
leafNode.Insert(leafNode.Count, new Element(items.Current.Key, items.Current.Value));
counter++;
more = items.MoveNext();
}
_storage.Storage.Update(handle.StoreHandle, _storage.NodeSerializer, leafNode);
leafNode.ToReadOnly();
if (!more && working.Count == 0)
{
return(leafNode);
}
InsertWorkingNode(handles, working, working.Count - 1, new Element(leafNode[0].Key, handle));
}
}
if (leafNode == null)
{
return(null);
}
if (leafNode.Count < _options.MinimumValueNodes)
{
working.Add(leafNode.CloneForWrite(LockType.Insert));
}
// Reballance of right-edge
for (int i = 1; i < working.Count; i++)
{
Node me = working[i];
bool isleaf = me.IsLeaf;
int limitMin = isleaf ? _options.MinimumValueNodes : _options.MinimumChildNodes;
if (me.Count < limitMin)
{
Node parent = working[i - 1];
int prev = parent.Count - 2;
Node prevNode;
bool success = _storage.Storage.TryGetNode(parent[prev].ChildNode.StoreHandle,
out prevNode, _storage.NodeSerializer);
AssertionFailedException.Assert(success);
prevNode = prevNode.CloneForWrite(LockType.Insert);
if (!isleaf)
{
me.ReplaceKey(0, parent[parent.Count - 1].Key);
}
while (me.Count < limitMin)
{
Element item = prevNode[prevNode.Count - 1];
if (me.Count + 1 == limitMin)
{
if (isleaf)
{
me.Insert(0, item);
}
else
{
me.Insert(0, new Element(default(TKey), item.ChildNode));
}
parent.ReplaceKey(parent.Count - 1, item.Key);
}
else
{
me.Insert(0, item);
}
prevNode.Remove(prevNode.Count - 1, item, _keyComparer);
}
_storage.Storage.Update(prevNode.StorageHandle, _storage.NodeSerializer, prevNode);
prevNode.ToReadOnly();
}
}
// Save the remaining nodes
for (int i = working.Count - 1; i >= 0; i--)
{
_storage.Storage.Update(working[i].StorageHandle, _storage.NodeSerializer, working[i]);
working[i].ToReadOnly();
}
return(working[0]);
}
private void SubscribeToMap(string topic)
{
lock (this)
{
if (imagehandle == null)
imagehandle = new NodeHandle();
if (mapSub != null && mapSub.topic != topic)
{
mapSub.shutdown();
mapSub = null;
}
if (mapSub != null)
return;
Console.WriteLine("Subscribing to map at:= " + topic);
mapSub = imagehandle.subscribe<nm.OccupancyGrid>(topic, 1, i => Dispatcher.Invoke(new Action(() =>
{
mapResolution = i.info.resolution;
mapHeight = i.info.height;
mapWidth = i.info.width;
if (Width != 0)
actualResolution = (mapWidth/(float) Width)*mapResolution;
else
actualResolution = (mapWidth / (float)ActualWidth) * mapResolution;
if (float.IsNaN(actualResolution) || float.IsInfinity(actualResolution))
actualResolution = 0;
else
{
MatchAspectRatio();
}
origin = new Point(i.info.origin.position.x, i.info.origin.position.y);
Size size = new Size(i.info.width, i.info.height);
byte[] data = createRGBA(i.data);
mGenericImage.UpdateImage(data, size, false);
data = null;
})));
}
}
InputPortID ITaskPort <ISpeed> .GetPort(NodeHandle handle) => (InputPortID)SimulationPorts.SpeedPort;
