private void UpdateHandsMovement()
{
this.handsdMotorUpdateCounter += 1;
if (this.handsdMotorUpdateCounter % 30 != 0)
{
return;
}
Vector3 leftHand = OVRInput.GetLocalControllerPosition(OVRInput.Controller.LTouch);
double angleXYLeft = GetAngleFromCoordinate(leftHand.x, leftHand.y, leftShoulder.x, leftShoulder.y);
double angleYZLeft = GetAngleFromCoordinate(leftHand.y, leftHand.z, leftShoulder.y, leftShoulder.z);
ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("p left_arm_v " + angleXYLeft));
//ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("p left_arm_v " + angleYZLeft));
Debug.Log("angle XY " + angleXYLeft);
Debug.Log("angle YZ " + angleYZLeft);
//Debug.Log("z " + leftHand.z * 100);
Vector3 rightHand = OVRInput.GetLocalControllerPosition(OVRInput.Controller.RTouch);
double angleXYRight = GetAngleFromCoordinate(rightHand.x, rightHand.y, rightShoulder.x, rightShoulder.y);
double angleYZRight = GetAngleFromCoordinate(rightHand.y, rightHand.z, rightShoulder.y, rightShoulder.z);
//ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("p right_arm_h " + rightHand.x*100));
//ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("p right_arm_v " + rightHand.y*100));
}
private void UpdateHeadMotor()
{
this.headMotorUpdateCounter += 1;
if (this.headMotorUpdateCounter % 30 != 0)
{
return;
}
//Debug.Log("camera rotation "+this.camera.transform.rotation.eulerAngles.y);
//Debug.Log("camera rotation " + this.camera.transform.localRotation.eulerAngles.y);
int rotation = (int)this.camera.transform.rotation.eulerAngles.y;
if (rotation >= 180)
{
rotation -= 360;
}
rotation *= 100 / 90;
if (rotation > 100)
{
rotation = 100;
}
if (rotation < -100)
{
rotation = -100;
}
Debug.Log("motor : " + rotation);
ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("p neck_h " + -rotation));
/*if (Input.GetKeyDown(KeyCode.D)) {
* Debug.Log("i neck_h 5");
* ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("i neck_h -5"));
* Debug.Log("ok");
*
* }
* if (Input.GetKeyDown(KeyCode.Q)) {
* Debug.Log("i neck_h -5");
* ros.Publish(MotorPublisher.GetMessageTopic(), new StringMsg("i neck_h 5"));
* Debug.Log("ok");
*
* }*/
}
// 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();
}
