/* Send sensor data as feedback to the control algorithm. The data sent includes the orientation of the vehicle, * acceleration in all three dimensions, depth under water, and forward velocity in local frame. * */ void SendData() { try { Vector3 CurRot = transform.parent.transform.rotation.eulerAngles; if (CurRot.x > 180.0f) { CurRot.x -= 360.0f; } if (CurRot.y > 180.0f) { CurRot.y -= 360.0f; } if (CurRot.z > 180.0f) { CurRot.z -= 360.0f; } //CurRot *= (float)Math.PI / 180.0f; Vector3 curVelocity = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().velocity); Vector3 CurAcc = (curVelocity - prevVelocity) / Time.deltaTime; prevVelocity = curVelocity; Vector3 Omega = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().angularVelocity); prevRot = CurRot; float modifiedDepth = (-transform.parent.position.y * 15.0f);//+930.0f; #region for old controller //Uncomment for old controller float[] angular = new float[] { -CurRot.x, CurRot.z, CurRot.y }; float[] linear = new float[] { CurAcc.x, -CurAcc.z, -CurAcc.y }; float depth = modifiedDepth; msg = new CombinedMsg(angular, linear, depth); //Debug.Log("combined message : "+msg); #endregion #region for new controller //For new controller // float[] velocity = new float[]{curVelocity.x, -curVelocity.z, -curVelocity.y}; //float[] acceleration = new float[]{CurAcc.x, -CurAcc.z, -CurAcc.y}; //float[] angle = new float[]{-CurRot.x, CurRot.z, CurRot.y}; //float[] omega = new float[]{-Omega.x, Omega.z, Omega.y}; //msg = new Ctrl_InputMsg(velocity, acceleration, angle, omega, modifiedDepth); #endregion #if notSelf obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(ROSPublisher.GetMessageTopic(), msg); #endif } catch (Exception e) { Debug.Log("Socket error: " + e); } }
/* Sets up a TCP socket connection with the server running on the machine runing * the control algorithm. * */ // public IEnumerator SetupSocket() // { // Debug.Log ("Setting the socket up"); // yield return null; // try{ // #if notSelf // mySocket = new TcpClient (IPAddress.Parse(IPaddress).ToString(), Port); // theStream = mySocket.GetStream(); // // theWriter = new StreamWriter(theStream); // // #endif // Ready = true; // } // catch (Exception e) { // Debug.Log("Socket error: " + e); // } // } /* Send sensor data as feedback to the control algorithm. The data sent includes the orientation of the vehicle, * acceleration in all three dimensions, depth under water, and forward velocity in local frame. * */ void SendData() { try { Vector3 CurRot = transform.parent.transform.rotation.eulerAngles; // Debug.Log("The angles are: " + CurRot); if (CurRot.x > 180.0f) { CurRot.x -= 360.0f; } if (CurRot.y > 180.0f) { CurRot.y -= 360.0f; } if (CurRot.z > 180.0f) { CurRot.z -= 360.0f; } CurRot *= (float)Math.PI / 180.0f; Vector3 curVelocity = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().velocity); Vector3 CurAcc = (curVelocity - prevVelocity) / Time.deltaTime; prevVelocity = curVelocity; // Vector3 Omega = (CurRot - prevRot)/Time.deltaTime; Vector3 Omega = transform.parent.transform.InverseTransformVector(transform.parent.GetComponent <Rigidbody>().angularVelocity); prevRot = CurRot; // string temp = (-CurRot.x).ToString("+000.00;-000.00") + " " + (-CurRot.z).ToString("+000.00;-000.00") + " " // + (CurRot.y).ToString("+000.00;-000.00") + " " + CurAcc.x.ToString("+000.00;-000.00") + " " // + CurAcc.z.ToString("+000.00;-000.00") + " " + (-CurAcc.y).ToString("+000.00;-000.00") + " " // + (-transform.parent.position.y).ToString("+000.00;-000.00") + " " // + transform.parent.GetComponent<Rigidbody>().velocity.x.ToString("+000.00;-000.00") + " $"; // Debug.Log(temp); float modifiedDepth = (-transform.parent.position.y * 15.0f) + 930.0f; //Uncomment for old controller // float[] angular = new float[]{-CurRot.x, CurRot.z, CurRot.y}; // float[] linear = new float[]{CurAcc.x, -CurAcc.z, modifiedDepth}; // // msg = new CombinedMsg(angular, linear); //For new controller float[] velocity = new float[] { curVelocity.x, -curVelocity.z, -curVelocity.y }; float[] acceleration = new float[] { CurAcc.x, -CurAcc.z, -CurAcc.y }; float[] angle = new float[] { -CurRot.x, CurRot.z, CurRot.y }; float[] omega = new float[] { -Omega.x, Omega.z, Omega.y }; msg = new Ctrl_InputMsg(velocity, acceleration, angle, omega, modifiedDepth); #if notSelf Debug.Log("Sending: Depth = " + modifiedDepth); Debug.Log("Sending: Force = " + CurAcc.y * 15000); Debug.Log("Sending to topic: " + ROSPublisher.GetMessageTopic()); obj.GetComponent <ROS_Initialize> ().ros.Publish(ROSPublisher.GetMessageTopic(), msg); #endif } catch (Exception e) { Debug.Log("Socket error: " + e); } }