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 = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort); this.networkStream = this.tcpClient.GetStream(); this.networkStream.ReadTimeout = 100000; this.networkStream.WriteTimeout = 100000; this.transformStampedMsg = new SIGVerseROSBridgeMessage <TransformStamped[]>("publish", this.topicName, "sigverse/TfList", null); }
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 = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort); 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); }
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.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort); this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort); 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.xtionRGBCamera = this.rgbCamera.GetComponentInChildren <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 = { 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); // [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.rgbCamera.name); 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 void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isRight) { if (rosBridgeIP.Equals(string.Empty)) { rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP; } if (sigverseBridgePort == 0) { sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort; } this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort); this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort); 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); }