IEnumerator SendImu()
{
yield return(new WaitForEndOfFrame());
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, "imu");
Quaternion vehicleImu;
vehicleImu = vehicle.transform.rotation;
double xey = vehicleImu.x;
double yey = vehicleImu.y;
double zey = vehicleImu.z;
double wey = vehicleImu.w;
var imuData = new QuaternionMsg(xey, yey, zey, wey);
double[] arg3 = { 0.00, 0.00, 0.00 };
var vect3 = new Vector3Msg(0.00, 0.00, 0.00);
var imumessage = new ImuMessage(headerMessage, imuData, arg3, vect3, arg3, vect3, arg3);
ros.Publish(imuPublisher.GetMessageTopic(), imumessage);
ros.Render();
}
public new static void CallBack(ROSBridgeMsg msg)
{
//Debug.Log (GetMessageTopic () + " received");
ImuMessage orientation = (ImuMessage)msg;
GameObject capsule = GameObject.Find("Capsule");
if (capsule == null)
{
Debug.Log("Cant Find AUV");
}
else
{
float x = (float)orientation.GetQuaternionIMU().GetX();
float y = (float)orientation.GetQuaternionIMU().GetY();
float z = (float)orientation.GetQuaternionIMU().GetZ();
float w = (float)orientation.GetQuaternionIMU().GetW();
capsule.transform.rotation = new Quaternion(x, y, z, w);
}
}
public static string ToYAMLString(ImuMessage msg)
{
return(msg.ToYAMLString());
}
