IEnumerator SendImage()
{
yield return(new WaitForEndOfFrame());
int width = 640;
int height = 480;
Texture2D tex = new Texture2D(width, height, TextureFormat.RGB24, false);
tex.ReadPixels(new Rect(0, 0, width, height), 0, 0);
tex.Apply();
byte[] data = tex.EncodeToJPG();
var now = DateTime.Now;
var timeSpan = now - lastFrame;
var timeSinceStart = now - camStart;
var timeMessage = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
var headerMessage = new HeaderMsg(count, timeMessage, "camera");
Debug.Log("data length: " + data.Length);
string format = "jpeg";
var compressedImageMsg = new CompressedImageMsg(headerMessage, format, data);
Debug.Log(compressedImageMsg);
ros.Publish(CompressedImagePublisher.GetMessageTopic(), compressedImageMsg);
Destroy(tex);
ros.Render();
}
public static Texture2D ToTexture2D(this CompressedImageMsg message)
{
var tex = new Texture2D(1, 1);
tex.LoadImage(message.data);
return(tex);
}
public void PublishData(Texture2D texture2D, int qualityLevel, int sequenceId)
{
HeaderMsg header = new HeaderMsg(sequenceId, new TimeMsg(0, 0), "raspicam");
CompressedImageMsg imageMsg = new CompressedImageMsg(header, "jpg", texture2D.EncodeToJPG(qualityLevel));
_publisher.PublishData(imageMsg);
}
public new static void CallBack(ROSBridgeMsg msg)
{
CompressedImageMsg imgMsg = msg as CompressedImageMsg;
//HUD.droneCamera.LoadImage(imgMsg.GetImage());
ROSManager.getInstance().getdroneCam().LoadImage(imgMsg.GetImage());
}
IEnumerator UploadPNG()
{
UnityEngine.Debug.LogWarning("InsideUploadPNG");
while (true)
{
if (!isLocalPlayer)
{
UnityEngine.Debug.Log("not the local player");
yield return(null);
}
// Debug.LogWarning("InsideUploadPNG");
yield return(new WaitForEndOfFrame());
if (!isLocalPlayer)
{
UnityEngine.Debug.Log("not the local player");
yield return(null);
}
while (!start_flag)
{
yield return(null);
}
//RenderTexture targetTexture;
// Texture2D texture2D = new Texture2D (width, height, TextureFormat.RGB24, true);
cam.Render();
cam.targetTexture = targetTexture;
Matrix4x4 m = cam.projectionMatrix;
UnityEngine.Debug.Log(m);
yield return(new WaitForEndOfFrame());
if (!isLocalPlayer)
{
UnityEngine.Debug.Log("not the local player");
yield return(null);
}
RenderTexture.active = targetTexture;
// yield return new WaitForEndOfFrame();
texture2D.ReadPixels(new Rect(0, 0, width, height), 0, 0);
// RenderTexture.active = null;
byte[] image = texture2D.EncodeToJPG();
// yield return new WaitForEndOfFrame();
CompressedImageMsg msg = new CompressedImageMsg(new HeaderMsg(0, new TimeMsg(0, 0), "cam"), "jpeg", image);
// yield return new WaitForEndOfFrame();
//ros.Publish(CompressedImageMsg.GetMessageTopic(), msg);
string image_topic = string.Concat(namsp, "/image/compressed");
UnityEngine.Debug.Log(image_topic);
ros.Publish(image_topic, msg);
// targetTexture = null;
// texture2D = null;
msg = null;
image = null;
// yield return null;
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
Debug.Log(GetMessageTopic() + " received");
CompressedImageMsg imageMsg = (CompressedImageMsg)msg;
Texture2D tex = new Texture2D(2, 2);
byte[] image = imageMsg.GetImage();
tex.LoadImage(image);
GameObject sphere = GameObject.Find("/SkySphere");
if (sphere == null)
{
Debug.Log("No /SkySpheree");
}
else
{
Texture2D.DestroyImmediate(sphere.renderer.material.mainTexture, true);
sphere.renderer.material.mainTexture = tex;
Debug.Log("Texture updated");
}
GameObject cube = GameObject.Find("/Cube");
if (sphere == null)
{
Debug.Log("No /Cube");
}
else
{
Texture2D.DestroyImmediate(cube.renderer.material.mainTexture, true);
cube.renderer.material.mainTexture = tex;
cube.transform.Rotate(0, Time.deltaTime * 50, 0, Space.World);
Debug.Log("Cube Texture updated");
Debug.Log("delta was " + Time.deltaTime);
}
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
Debug.Log(GetMessageTopic() + " received");
CompressedImageMsg imageMsg = (CompressedImageMsg)msg;
Debug.Log("HERE IS THE BYTE[] COMMING IN FROM ROS");
Debug.Log("------------------------------------------");
byte[] arr;
arr = imageMsg.GetImage();
Debug.Log(imageMsg.GetImage());
Debug.Log(arr[1]);
Debug.Log("------------------------------------------");
byte[] bimage = imageMsg.GetImage();
texture.LoadImage(bimage);
//texture.LoadRawTextureData(bimage);
texture.Apply();
// GUI.DrawTexture(new Rect(10, 10, 60, 60), texture, ScaleMode.ScaleToFit, true, 10.0F);
//GetComponent<Renderer>().material.mainTexture = texture;
}
IEnumerator SendImages()
{
while (true)
{
Texture2D rgb = RTImageColor(_cameras[0]);
Texture2D depth = RTImageDepth(_cameras[1]);
//Color c;
//for (int i = 0; i < rgb.width; i++) {
// for (int j = 0; j < rgb.height; j++) {
// c = rgb.GetPixel(i, j);
// c.a = depth.GetPixel(i, j).a;
// rgb.SetPixel(i, j, c);
// }
//}
Color[] pxs = rgb.GetPixels();
Color[] pxsDepth = depth.GetPixels();
for (int i = 0; i < pxs.Length; i++)
{
pxs[i].a = pxsDepth[i].a;
}
rgb.SetPixels(pxs);
HeaderMsg _head = new HeaderMsg(0, new TimeMsg(_ros.GetepochStart().Second, 0), "map");
//RGB
CompressedImageMsg compressedImg = new CompressedImageMsg(_head, "png", rgb.EncodeToPNG());
_ros.Publish(RobotCamera_pub.GetMessageTopic(), compressedImg);
//Depth
//compressedImg = new CompressedImageMsg(_head, "png", RTImageDepth(_cameras[1]).EncodeToPNG());
//_ros.Publish(CameraDepth_pub.GetMessageTopic(), compressedImg);
yield return(new WaitForSeconds(_frecuency));
}
}
public static string ToYAMLString(CompressedImageMsg msg)
{
return(msg.ToYAMLString());
}
// This function should fire on each ros message
public new static void CallBack(ROSBridgeMsg msg)
{
image = (CompressedImageMsg)msg;
}
// Update is called once per frame in Unity
void Update()
{
if (Input.GetKeyDown("d"))
{
ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("i neck_h 5"));
}
if (Input.GetKeyDown("q"))
{
ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("i neck_h -5"));
}
//int size = (int)System.Math.Sqrt(ImageSubscriber.image.GetImage().Length);
Texture2D texture = new Texture2D(2, 2);
CompressedImageMsg image = ImageSubscriber.image;
if (image != null)
{
texture.LoadImage(ImageSubscriber.image.GetImage());
texture.Apply();
sr.material.mainTexture = texture;
mySprite = Sprite.Create(texture, new Rect(0.0f, 0.0f, texture.width, texture.height), new Vector2(0.5f, 0.5f), 100.0f);
sr.sprite = mySprite;
}
else
{
//Debug.Log("no image stream");
}
AudioDataMsg audioMsg = AudioSubscriber.audio;
if (audioMsg != null)
{
/*
* AudioSource audioSource = GetComponent<AudioSource>();
* audioSource.volume = 1;
*
*
* string url = Application.dataPath + "/Scripts/test.wav";
* WWW www = new WWW(url);
*
* //audioSource.clip = www.GetAudioClip();
*
*
*
* //audioSource.Play();
* if (!audioSource.isPlaying) {
* AssetDatabase.ImportAsset("Assets/Resources/test.wav");
* AudioClip clip1 = Resources.Load<AudioClip>("test");
* //Debug.Log(clip1);
* //Debug.Log(clip1.length);
* audioSource.PlayOneShot(clip1);
* //Resources.UnloadAsset(clip1);
* //audioSource.Play();
* //Debug.Log("playing");
*
* }
*
*
*
* string strCmdText;
* //strCmdText = "/C copy /b Image1.jpg + Archive.rar Image2.jpg";
* //System.Diagnostics.Process.Start("CMD.exe", strCmdText);
*
* byte[] tmp;
* if (oldAudio != null) {
* tmp = new byte[audioMsg.GetAudio().Length + oldAudio.Length];
* oldAudio.CopyTo(tmp, 0);
* audioMsg.GetAudio().CopyTo(tmp, oldAudio.Length);
*
* counter += 1;
* }
* else {
* tmp = audioMsg.GetAudio();
* }
*
*
* if(counter == 20000) {
* using (FileStream fs = new FileStream("Assets/MySound2.wav", FileMode.Create)) {
* //WriteHeader(fs, tmp.Length, 1, 16000);
*
* /*MemoryStream stream = new MemoryStream();
*
* stream.Write(tmp, 0, tmp.Length);
*
* WriteWavHeader(stream, false, 1, 16, 16000, tmp.Length);
*
* byte[] bytesArray = stream.ToArray();
* float[] floatArray = ConvertByteToFloat(bytesArray);
*
* AudioSource audioSource = GetComponent<AudioSource>();
* AudioClip audioClip = AudioClip.Create("testSound", floatArray.Length, 1, 16000, false);
* audioClip.SetData(floatArray, 0);
*
* audioSource.clip = audioClip;
* audioSource.Play();
*
* fs.Write(bytesArray, 0, bytesArray.Length);
* fs.Close();
*
* File.WriteAllBytes("Assets/Scripts/test.tmp", tmp);
* Debug.Log("written to disk");
* Debug.Log(tmp.GetValue(100));
* Debug.Log(tmp.GetValue(200));
* Debug.Log(tmp.GetValue(300));
* Debug.Log(tmp.GetValue(600));
* Debug.Log(tmp.GetValue(800));
* }
*
* //AudioSource audioSource = GetComponent<AudioSource>();
* //audioSource.Play();
* }
*
* /*MemoryStream stream = new MemoryStream();
*
* stream.Write(audioMsg.GetAudio(), 0, audioMsg.GetAudio().Length);
*
* WriteWavHeader(stream, false, 1, 16, 16000, audioMsg.GetAudio().Length);
*
* byte[] bytesArray = stream.ToArray();
* float[] floatArray = ConvertByteToFloat(bytesArray);
*
* AudioSource audioSource = GetComponent<AudioSource>();
* AudioClip audioClip = AudioClip.Create("testSound", floatArray.Length, 1, 16000, false);
* audioClip.SetData(floatArray, 0);
*
* audioSource.clip = audioClip;
*
* if (!audioSource.isPlaying) {
* audioSource.Play();
* }
*/
//oldAudio = tmp;
//audioSource.Play(16000);
//AudioClip audioClip = AudioClip.Create("testSound", audio.Length, 1, 16000, false);
//audioClip.SetData(audio, 0);
//AudioSource.PlayClipAtPoint(audioClip, new Vector3(100, 100, 0), 1.0f);
/*WAV wav = new WAV(audioMsg.GetAudio());
* Debug.Log(wav);
* AudioClip audioClip = AudioClip.Create("testSound", wav.SampleCount, 1, wav.Frequency, false);
* audioClip.SetData(wav.LeftChannel, 0);
*
* AudioSource.PlayClipAtPoint(audioClip, new Vector3(100, 100, 0), 1.0f);*/
//audio.clip = audioClip;
//audio.Play();
}
else
{
//Debug.Log("no audio stream");
}
/*for (int y = 0; y < texture.height; y++) {
* for (int x = 0; x < texture.width; x++) {
* int r = ImageSubscriber.image.GetImage()[texture.width * y + x];
* Color color = new Color()
* Color color = ((x & y) != 0 ? Color.white : Color.gray);
* texture.SetPixel(x, y, color);
* }
* }*/
ros.Render();
}
public ROSCameraSensorPublisher(ROSBridgeWebSocketConnection rosConnection, string topicName)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, CompressedImageMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
// Update is called once per frame in Unity
void Update()
{
//Init the left shoulder with the right controller
if (GameManager.instance.InitPointLeft)
{
if (OVRInput.Get(OVRInput.Button.SecondaryHandTrigger))
{
GameManager.instance.InitPointLeft = false;
this.leftShoulder = OVRInput.GetLocalControllerPosition(OVRInput.Controller.RTouch);
Debug.Log("Left shoulder : " + leftShoulder);
}
}
//Init the right shoulder with the left controller
if (GameManager.instance.InitPointRight)
{
if (OVRInput.Get(OVRInput.Button.PrimaryHandTrigger))
{
GameManager.instance.InitPointRight = false;
this.rightShoulder = OVRInput.GetLocalControllerPosition(OVRInput.Controller.LTouch);
Debug.Log("Right shoulder : " + rightShoulder);
}
}
//Debug.Log("New frame :");
// And in some other class where the ball is controlled:
//TwistMsg msg = new TwistMsg(new Vector3Msg(10, 20, 30), new Vector3Msg(0, 0, 0)); // Circa
// Publish it (ros is the object defined in the first class)
//ros.Publish(BallControlPublisher.GetMessageTopic(), msg);
//ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("i neck_h 5"));
//int size = (int)System.Math.Sqrt(ImageSubscriber.image.GetImage().Length);
//For unloading the previous image
//DestroyImmediate(this.image.sprite.texture, true);
Texture2D texture = new Texture2D(2, 2);
CompressedImageMsg image = ImageSubscriber.image;
if (image != null)
{
this.RemoveText();
texture.LoadImage(ImageSubscriber.image.GetImage());
texture.Apply();
//sr.material.mainTexture = texture;
mySprite = Sprite.Create(texture, new Rect(0.0f, 0.0f, texture.width, texture.height), new Vector2(0.5f, 0.5f), 100.0f);
//sr.sprite = mySprite;
this.image.sprite = mySprite;
}
else
{
//Debug.Log("no image stream");
}
if (DepthSubscriber.cloud != null)
{
//Debug.Log("wtf");
//Debug.Log(DepthSubscriber.cloud);
List <PointXYZRGB> points = (List <PointXYZRGB>)DepthSubscriber.cloud.GetCloud().Points;
//Debug.Log(points.Count);
//foreach (var point in points) {
//Debug.Log(point.R + " | " + point.G + " | " + point.B + " ----- " + point.X + " | " + point.Y + " | " + point.Z);
//}
}
else
{
//Debug.Log("no cloud stream weird");
}
//AudioDataMsg audioMsg = AudioSubscriber.audio;
//if(audioMsg != null) {
/*
* AudioSource audioSource = GetComponent<AudioSource>();
* audioSource.volume = 1;
*
*
* string url = Application.dataPath + "/Scripts/test.wav";
* WWW www = new WWW(url);
*
* //audioSource.clip = www.GetAudioClip();
*
*
*
* //audioSource.Play();
* if (!audioSource.isPlaying) {
* AssetDatabase.ImportAsset("Assets/Resources/test.wav");
* AudioClip clip1 = Resources.Load<AudioClip>("test");
* //Debug.Log(clip1);
* //Debug.Log(clip1.length);
* audioSource.PlayOneShot(clip1);
* //Resources.UnloadAsset(clip1);
* //audioSource.Play();
* //Debug.Log("playing");
*
* }
*
*
*
* string strCmdText;
* //strCmdText = "/C copy /b Image1.jpg + Archive.rar Image2.jpg";
* //System.Diagnostics.Process.Start("CMD.exe", strCmdText);
*
* byte[] tmp;
* if (oldAudio != null) {
* tmp = new byte[audioMsg.GetAudio().Length + oldAudio.Length];
* oldAudio.CopyTo(tmp, 0);
* audioMsg.GetAudio().CopyTo(tmp, oldAudio.Length);
*
* counter += 1;
* }
* else {
* tmp = audioMsg.GetAudio();
* }
*
*
* if(counter == 20000) {
* using (FileStream fs = new FileStream("Assets/MySound2.wav", FileMode.Create)) {
* //WriteHeader(fs, tmp.Length, 1, 16000);
*
* /*MemoryStream stream = new MemoryStream();
*
* stream.Write(tmp, 0, tmp.Length);
*
* WriteWavHeader(stream, false, 1, 16, 16000, tmp.Length);
*
* byte[] bytesArray = stream.ToArray();
* float[] floatArray = ConvertByteToFloat(bytesArray);
*
* AudioSource audioSource = GetComponent<AudioSource>();
* AudioClip audioClip = AudioClip.Create("testSound", floatArray.Length, 1, 16000, false);
* audioClip.SetData(floatArray, 0);
*
* audioSource.clip = audioClip;
* audioSource.Play();
*
* fs.Write(bytesArray, 0, bytesArray.Length);
* fs.Close();
*
* File.WriteAllBytes("Assets/Scripts/test.tmp", tmp);
* Debug.Log("written to disk");
* Debug.Log(tmp.GetValue(100));
* Debug.Log(tmp.GetValue(200));
* Debug.Log(tmp.GetValue(300));
* Debug.Log(tmp.GetValue(600));
* Debug.Log(tmp.GetValue(800));
* }
*
* //AudioSource audioSource = GetComponent<AudioSource>();
* //audioSource.Play();
* }
*
* /*MemoryStream stream = new MemoryStream();
*
* stream.Write(audioMsg.GetAudio(), 0, audioMsg.GetAudio().Length);
*
* WriteWavHeader(stream, false, 1, 16, 16000, audioMsg.GetAudio().Length);
*
* byte[] bytesArray = stream.ToArray();
* float[] floatArray = ConvertByteToFloat(bytesArray);
*
* AudioSource audioSource = GetComponent<AudioSource>();
* AudioClip audioClip = AudioClip.Create("testSound", floatArray.Length, 1, 16000, false);
* audioClip.SetData(floatArray, 0);
*
* audioSource.clip = audioClip;
*
* if (!audioSource.isPlaying) {
* audioSource.Play();
* }
*/
//oldAudio = tmp;
//audioSource.Play(16000);
//AudioClip audioClip = AudioClip.Create("testSound", audio.Length, 1, 16000, false);
//audioClip.SetData(audio, 0);
//AudioSource.PlayClipAtPoint(audioClip, new Vector3(100, 100, 0), 1.0f);
/*WAV wav = new WAV(audioMsg.GetAudio());
* Debug.Log(wav);
* AudioClip audioClip = AudioClip.Create("testSound", wav.SampleCount, 1, wav.Frequency, false);
* audioClip.SetData(wav.LeftChannel, 0);
*
* AudioSource.PlayClipAtPoint(audioClip, new Vector3(100, 100, 0), 1.0f);*/
//audio.clip = audioClip;
//audio.Play();
//}
/*for (int y = 0; y < texture.height; y++) {
* for (int x = 0; x < texture.width; x++) {
* int r = ImageSubscriber.image.GetImage()[texture.width * y + x];
* Color color = new Color()
* Color color = ((x & y) != 0 ? Color.white : Color.gray);
* texture.SetPixel(x, y, color);
* }
* }*/
ros.Render();
}
