예제 #1
0
        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);
        }
예제 #2
0
        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;
        }
예제 #3
0
        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);
        }
        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;
        }
예제 #5
0
        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 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;
        }
예제 #9
0
        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;
        }
예제 #10
0
        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;
        }