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