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;