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);
}
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;
}
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;
}
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;
}
