void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClient = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStream = this.tcpClient.GetStream();
this.networkStream.ReadTimeout = 100000;
this.networkStream.WriteTimeout = 100000;
this.transformStampedMsg = new SIGVerseROSBridgeMessage <TransformStamped[]>("publish", this.topicName, "sigverse/TfList", null);
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClient = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStream = this.tcpClient.GetStream();
this.networkStream.ReadTimeout = 100000;
this.networkStream.WriteTimeout = 100000;
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.sensorLink.name);
this.laserScan = new LaserScanForSIGVerseBridge();
this.numLines = (int)Mathf.Round(LaserAngle / LaserAngularResolution) + 1;
this.laserScan.header = this.header;
this.laserScan.angle_min = -HalfOfLaserAngle * Mathf.Deg2Rad;
this.laserScan.angle_max = +HalfOfLaserAngle * Mathf.Deg2Rad;
this.laserScan.angle_increment = LaserAngularResolution * Mathf.Deg2Rad;
this.laserScan.time_increment = 0.0;
this.laserScan.scan_time = 0.1; // tentative
this.laserScan.range_min = 0.05;
this.laserScan.range_max = 20.0;
this.laserScan.ranges = new double[this.numLines];
this.laserScan.intensities = new double[this.numLines];
this.laserScanMsg = new SIGVerseROSBridgeMessage <LaserScanForSIGVerseBridge>("publish", this.topicName, LaserScanForSIGVerseBridge.GetMessageType(), this.laserScan);
// Debug.Log("this.layerMask.value = "+this.layerMask.value);
}
void Start()
{
if (this.rosBridgeIP.Equals(string.Empty))
{
this.rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (this.sigverseBridgePort == 0)
{
this.sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(this.rosBridgeIP, this.sigverseBridgePort);
this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream();
this.networkStreamCameraInfo.ReadTimeout = 100000;
this.networkStreamCameraInfo.WriteTimeout = 100000;
this.networkStreamImage = this.tcpClientImage.GetStream();
this.networkStreamImage.ReadTimeout = 100000;
this.networkStreamImage.WriteTimeout = 100000;
// RGB Camera
this.xtionRGBCamera = this.rgbCamera.GetComponentInChildren <Camera>();
int imageWidth = this.xtionRGBCamera.targetTexture.width;
int imageHeight = this.xtionRGBCamera.targetTexture.height;
this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);
// [camera/rgb/CameraInfo]
string distortionModel = "plumb_bob";
double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] K = { 554, 0.0, 320, 0.0, 554, 240, 0.0, 0.0, 1.0 };
double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
double[] P = { 554, 0.0, 320, 0.0, 0.0, 554, 240, 0.0, 0.0, 0.0, 1.0, 0.0 };
//double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
//double[] K = { 554.3827128226441, 0.0, 320.5, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 1.0 };
//double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
//double[] P = { 554.3827128226441, 0.0, 320.5, 0.0, 0.0, 554.3827128226441, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);
this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);
// [camera/rgb/Image_raw]
string encoding = "rgb8";
byte isBigendian = 0;
uint step = (uint)imageWidth * 3;
this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.rgbCamera.name);
this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", this.topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
this.imageMsg = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge> ("publish", this.topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
}
public void Initialize(string rosBridgeIP, int sigverseBridgePort, string topicNameCameraInfo, string topicNameImage, bool isRight)
{
if (rosBridgeIP.Equals(string.Empty))
{
rosBridgeIP = ConfigManager.Instance.configInfo.rosbridgeIP;
}
if (sigverseBridgePort == 0)
{
sigverseBridgePort = ConfigManager.Instance.configInfo.sigverseBridgePort;
}
this.tcpClientCameraInfo = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);
this.tcpClientImage = HSRCommon.GetSIGVerseRosbridgeConnection(rosBridgeIP, sigverseBridgePort);
this.networkStreamCameraInfo = this.tcpClientCameraInfo.GetStream();
this.networkStreamCameraInfo.ReadTimeout = 100000;
this.networkStreamCameraInfo.WriteTimeout = 100000;
this.networkStreamImage = this.tcpClientImage.GetStream();
this.networkStreamImage.ReadTimeout = 100000;
this.networkStreamImage.WriteTimeout = 100000;
// RGB Camera
this.rgbCamera = this.cameraFrameObj.GetComponentInChildren <Camera>();
int imageWidth = this.rgbCamera.targetTexture.width;
int imageHeight = this.rgbCamera.targetTexture.height;
this.imageTexture = new Texture2D(imageWidth, imageHeight, TextureFormat.RGB24, false);
// [camera/rgb/CameraInfo]
string distortionModel = "plumb_bob";
double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
double[] K = { 968.765, 0.0, 640, 0.0, 968.77, 480, 0.0, 0.0, 1.0 };
double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
double[] P = { 968.765, 0.0, 640, 0.0, 0.0, 968.77, 480, 0.0, 0.0, 0.0, 1.0, 0.0 };
if (isRight)
{
P[3] = -135.627; // -135.627 = - 968.765 * 0.14(baseline=distance between both eyes)
}
//double[] D = { 0.0, 0.0, 0.0, 0.0, 0.0 };
//double[] K = { 968.7653251755174, 0.0, 640.5, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 1.0 };
//double[] R = { 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0 };
// Left Camera
//double[] P = { 968.7653251755174, 0.0, 640.5, -0.0, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
// Right Camera
//double[] P = { 968.7653251755174, 0.0, 640.5, -135.62714552457246, 0.0, 968.7653251755174, 480.5, 0.0, 0.0, 0.0, 1.0, 0.0 };
RegionOfInterest roi = new RegionOfInterest(0, 0, 0, 0, false);
this.cameraInfoData = new CameraInfoForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, distortionModel, D, K, R, P, 0, 0, roi);
// [camera/rgb/Image_raw]
string encoding = "rgb8";
byte isBigendian = 0;
uint step = (uint)imageWidth * 3;
this.imageData = new ImageForSIGVerseBridge(null, (uint)imageHeight, (uint)imageWidth, encoding, isBigendian, step, null);
this.header = new Header(0, new SIGVerse.ROSBridge.msg_helpers.Time(0, 0), this.cameraFrameObj.name);
this.cameraInfoMsg = new SIGVerseROSBridgeMessage <CameraInfoForSIGVerseBridge>("publish", topicNameCameraInfo, CameraInfoForSIGVerseBridge.GetMessageType(), this.cameraInfoData);
this.imageMsg = new SIGVerseROSBridgeMessage <ImageForSIGVerseBridge> ("publish", topicNameImage, ImageForSIGVerseBridge.GetMessageType(), this.imageData);
}
