void Start() { if (this.rosbridgeIP.Equals(string.Empty)) { this.rosbridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP; } if (this.sigverseBridgePort == 0) { this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort; } this.tcpClient = new System.Net.Sockets.TcpClient(this.rosbridgeIP, this.sigverseBridgePort); this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; // Depth Camera this.xtionDepthCamera = SIGVerseUtils.FindTransformFromChild(this.transform.root, "camera_depth_optical_frame").GetComponentInChildren <Camera>(); int imageWidth = this.xtionDepthCamera.targetTexture.width; int imageHeight = this.xtionDepthCamera.targetTexture.height; this.byteArray = new byte[imageWidth * imageHeight * 2]; for (int i = 0; i < this.byteArray.Length; i++) { this.byteArray[i] = 0; } this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/depth/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 570.3422241210938, 0.0, 314.5, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 570.3422241210938, 0.0, 314.5, 0.0, 0.0, 570.3422241210938, 235.5, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/depth/Image_raw] string encoding = "16UC1"; byte isBigendian = 0; uint step = (uint)imageWidth * 2; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), "camera_depth_optical_frame"); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); }
public virtual void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // Camera this.targetCamera = this.GetComponent <Camera>(); int imageWidth = this.targetCamera.targetTexture.width; int imageHeight = this.targetCamera.targetTexture.height; this.imageTexture = new Texture2D(imageWidth, imageHeight, this.textureFormat, false); // [CameraInfo] this.cameraInfoData = this.InitializeCameraInfo((uint)imageHeight, (uint)imageWidth); // [Image_raw] this.imageData = this.InitializeImage((uint)imageHeight, (uint)imageWidth); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
void Start() { if (this.rosbridgeIP.Equals(string.Empty)) { this.rosbridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP; } if (this.sigverseBridgePort == 0) { this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort; } this.tcpClient = new System.Net.Sockets.TcpClient(this.rosbridgeIP, this.sigverseBridgePort); Debug.Log("Connected=" + this.tcpClient.Connected); this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; // RGB Camera this.xtionRGBCamera = this.transform.Find("Xtion_rgb").GetComponent <Camera>(); int imageWidth = this.xtionRGBCamera.targetTexture.width; int imageHeight = this.xtionRGBCamera.targetTexture.height; this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/rgb/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 570.3422241210938, 0.0, 319.5, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 570.3422241210938, 0.0, 319.5, 0.0, 0.0, 570.3422241210938, 239.5, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/rgb/Image_raw] string encoding = "rgb8"; byte isBigendian = 0; uint step = (uint)imageWidth * 3; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), "camera_rgb_optical_frame"); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); }
protected override void Start() { base.Start(); this.angleMin = -AngleInc * this.laserNum / 2; this.angleMax = +AngleInc * this.laserNum / 2 - AngleInc; if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicName)) { this.tcpClient = SIGVerseRosBridgeConnection.GetConnection(this.rosBridgeIP, this.sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicName, this.tcpClient); } else { this.tcpClient = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicName]; } this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.sensorLink.name); this.laserScan = new LaserScanForSIGVerseBridge(); this.laserScan.header = this.header; this.laserScan.angle_min = this.angleMin; this.laserScan.angle_max = this.angleMax; this.laserScan.angle_increment = AngleInc; this.laserScan.time_increment = 0.0; this.laserScan.scan_time = this.sendingInterval / 1000.0f; this.laserScan.range_min = RangeMin; this.laserScan.range_max = RangeMax; this.laserScan.ranges = new double[this.laserNum]; this.laserScan.intensities = new double[this.laserNum]; this.laserScanMsg = new SIGVerseRosBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan); // Debug.Log("AngleMin="+this.angleMin+", AngleMax="+this.angleMax+", AngleInc="+AngleInc); }
protected override void Start() { base.Start(); if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicName)) { this.tcpClient = SIGVerseRosBridgeConnection.GetConnection(this.rosBridgeIP, this.sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicName, this.tcpClient); } else { this.tcpClient = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicName]; } this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.sensorLink.name); this.laserScan = new LaserScanForSIGVerseBridge(); this.numLines = (int)Mathf.Round(LaserAngle / LaserAngularResolution) + 1; this.laserScan.header = this.header; this.laserScan.angle_min = -HalfOfLaserAngle * Mathf.Deg2Rad; this.laserScan.angle_max = +HalfOfLaserAngle * Mathf.Deg2Rad; this.laserScan.angle_increment = LaserAngularResolution * Mathf.Deg2Rad; this.laserScan.time_increment = 0.0; this.laserScan.scan_time = 0.1; // tentative this.laserScan.range_min = 0.05; this.laserScan.range_max = 20.0; this.laserScan.ranges = new double[this.numLines]; this.laserScan.intensities = new double[this.numLines]; this.laserScanMsg = new SIGVerseRosBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan); // Debug.Log("this.layerMask.value = "+this.layerMask.value); }
protected override void Start() { base.Start(); if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicName)) { this.tcpClient = SIGVerseRosBridgeConnection.GetConnection(this.rosBridgeIP, this.sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicName, this.tcpClient); } else { this.tcpClient = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicName]; } this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.transformStampedMsg = new SIGVerseRosBridgeMessage <TransformStamped[]>("publish", this.topicName, "sigverse/TfList", null); }
protected override void Start() { base.Start(); if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicName)) { this.tcpClient = SIGVerseRosBridgeConnection.GetConnection(this.rosBridgeIP, this.sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicName, this.tcpClient); } else { this.tcpClient = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicName]; } this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.sensorLink.name); this.laserScan = new LaserScanForSIGVerseBridge(); this.laserScan.header = this.header; this.laserScan.angle_min = AngleMin * Mathf.Deg2Rad; this.laserScan.angle_max = AngleMax * Mathf.Deg2Rad; this.laserScan.angle_increment = AngleIncrement * Mathf.Deg2Rad; this.laserScan.time_increment = 0.0; // (1.0f / (ScanRate / 60.0f)) / NumLines; this.laserScan.scan_time = 0.0; // 1.0f / (ScanRate / 60.0f); this.laserScan.range_min = RangeMin; this.laserScan.range_max = RangeMax; this.laserScan.ranges = new double[NumLines]; this.laserScan.intensities = new double[NumLines]; this.laserScanMsg = new SIGVerseRosBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan); }
protected override void Start() { base.Start(); if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicName)) { this.tcpClient = SIGVerseRosBridgeConnection.GetConnection(this.rosBridgeIP, this.sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicName, this.tcpClient); } else { this.tcpClient = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicName]; } this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.laserScanMsg = new SIGVerseRosBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan); // Debug.Log("this.layerMask.value = "+this.layerMask.value); }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // Depth Camera this.depthCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.depthCamera.targetTexture.width; int imageHeight = this.depthCamera.targetTexture.height; this.byteArray = new byte[imageWidth * imageHeight * 2]; for (int i = 0; i < this.byteArray.Length; i++) { this.byteArray[i] = 0; } this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/depth/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 465, 0.0, 320, 0.0, 465, 240, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 465, 0.0, 320, 0.0, 0.0, 465, 240, 0.0, 0.0, 0.0, 1.0, 0.0 }; // double[] D = { 0.14078746736049652, 0.07252906262874603, 0.004671256057918072, 0.0014421826926991343, 0.06731976568698883 }; // double[] K = { 475.25030517578125, 0.0, 333.3515625, 0.0, 475.2502136230469, 245.8830108642578, 0.0, 0.0, 1.0 }; // double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; // double[] P = { 475.25030517578125, 0.0, 333.3515625, 0.024700000882148743, 0.0, 475.2502136230469, 245.8830108642578, -0.0007332635577768087, 0.0, 0.0, 1.0, 0.004069563001394272 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/depth/Image_raw] string encoding = "16UC1"; byte isBigendian = 0; uint step = (uint)imageWidth * 2; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // RGB Camera this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.rgbCamera.targetTexture.width; int imageHeight = this.rgbCamera.targetTexture.height; this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/rgb/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 639, 0.0, 320, 0.0, 639, 240, 0.0, 0.0, 1.0 }; // memo: 639 = 465(depth) * tan(55/2) / tan(41.5/2) double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 639, 0.0, 320, 0.0, 0.0, 639, 240, 0.0, 0.0, 0.0, 1.0, 0.0 }; // double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; // double[] K = { 615.8720092773438, 0.0, 314.9148254394531, 0.0, 615.8720703125, 238.09365844726562, 0.0, 0.0, 1.0 }; // double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; // double[] P = { 615.8720092773438, 0.0, 314.9148254394531, 0.0, 0.0, 615.8720703125, 238.09365844726562, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/rgb/Image_raw] string encoding = "rgb8"; byte isBigendian = 0; uint step = (uint)imageWidth * 3; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isRight, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // RGB Camera this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.rgbCamera.targetTexture.width; int imageHeight = this.rgbCamera.targetTexture.height; this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/rgb/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 968.765, 0.0, 640, 0.0, 968.77, 480, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 968.765, 0.0, 640, 0.0, 0.0, 968.77, 480, 0.0, 0.0, 0.0, 1.0, 0.0 }; if (isRight) { P[3] = -135.627; // -135.627 = - 968.765 * 0.14(baseline=distance between both eyes) } //double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; //double[] K = { 968.7653251755174, 0.0, 640.5, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 1.0 }; //double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; // Left Camera //double[] P = { 968.7653251755174, 0.0, 640.5, -0.0, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 }; // Right Camera //double[] P = { 968.7653251755174, 0.0, 640.5, -135.62714552457246, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/rgb/Image_raw] string encoding = "rgb8"; byte isBigendian = 0; uint step = (uint)imageWidth * 3; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // RGB Camera this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.rgbCamera.targetTexture.width; int imageHeight = this.rgbCamera.targetTexture.height; this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/rgb/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 639, 0.0, 320, 0.0, 639, 240, 0.0, 0.0, 1.0 }; // memo: 639 = 445(depth) * tan(46/2) / tan(43/2) * 480/360 double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 639, 0.0, 320, 0.0, 0.0, 639, 240, 0.0, 0.0, 0.0, 1.0, 0.0 }; // double[] D = { -0.09601674973964691, 0.09204437583684921, -0.0017978776013478637, 0.0009506311034783721, 0.0 }; // double[] K = { 618.6206665039062, 0.0, 310.1082458496094, 0.0, 624.5146484375, 230.0232696533203, 0.0, 0.0, 1.0 }; // double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; // double[] P = { 618.6206665039062, 0.0, 310.1082458496094, 0.0, 0.0, 624.5146484375, 230.0232696533203, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/rgb/Image_raw] string encoding = "rgb8"; byte isBigendian = 0; uint step = (uint)imageWidth * 3; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // Camera this.targetCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.targetCamera.targetTexture.width; int imageHeight = this.targetCamera.targetTexture.height; this.byteArray = Enumerable.Repeat <byte>(0xFF, imageWidth * imageHeight * 2).ToArray(); this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 554, 0.0, 320, 0.0, 554, 240, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 554, 0.0, 320, 0.0, 0.0, 554, 240, 0.0, 0.0, 0.0, 1.0, 0.0 }; //double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; //double[] K = { 554.3827128226441, 0.0, 320.5, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 1.0 }; //double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; //double[] P = { 554.3827128226441, 0.0, 320.5, 0.0, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [Image_raw] string encoding = "16UC1"; byte isBigendian = 0; uint step = (uint)imageWidth * 2; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isUsingThread) { if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameCameraInfo)) { this.tcpClientCameraInfo = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameCameraInfo, this.tcpClientCameraInfo); } else { this.tcpClientCameraInfo = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameCameraInfo]; } if (!RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.ContainsKey(topicNameImage)) { this.tcpClientImage = SIGVerseRosBridgeConnection.GetConnection(rosBridgeIP, sigverseBridgePort); RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap.Add(topicNameImage, this.tcpClientImage); } else { this.tcpClientImage = RosConnectionManager.Instance.rosConnections.sigverseRosBridgeTcpClientMap[topicNameImage]; } this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream(); this.networkStreamCameraInfo.ReadTimeout = 100000; this.networkStreamCameraInfo.WriteTimeout = 100000; this.networkStreamImage = this.tcpClientImage.GetStream(); this.networkStreamImage.ReadTimeout = 100000; this.networkStreamImage.WriteTimeout = 100000; // Depth Camera this.depthCamera = this.cameraFrameObj.GetComponentInChildren <Camera>(); int imageWidth = this.depthCamera.targetTexture.width; int imageHeight = this.depthCamera.targetTexture.height; this.byteArray = new byte[imageWidth * imageHeight * 2]; for (int i = 0; i < this.byteArray.Length; i++) { this.byteArray[i] = 0; } this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false); // [camera/depth/CameraInfo] string distortionModel = "plumb_bob"; double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; double[] K = { 445, 0.0, 240, 0.0, 445, 180, 0.0, 0.0, 1.0 }; double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; double[] P = { 445, 0.0, 240, 0.0, 0.0, 445, 180, 0.0, 0.0, 0.0, 1.0, 0.0 }; // double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 }; // double[] K = { 455.84368896484375, 0.0, 225.90170288085938, 0.0, 455.84368896484375, 179.5, 0.0, 0.0, 1.0 }; // double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 }; // double[] P = { 455.84368896484375, 0.0, 225.90170288085938, -0.058707427233457565, 0.0, 455.84368896484375, 179.5, -0.00020851753652095795, 0.0, 0.0, 1.0, -0.0005153142847120762 }; RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false); this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi); // [camera/depth/Image_raw] string encoding = "16UC1"; byte isBigendian = 0; uint step = (uint)imageWidth * 2; this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null); this.header = new Header(0, new SIGVerse.RosBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name); this.cameraInfoMsg = new SIGVerseRosBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData); this.imageMsg = new SIGVerseRosBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData); this.isUsingThread = isUsingThread; }