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();
}
void Update()
{
ros.Render();
if (!_running)
{
return;
}
timer -= Time.deltaTime;
if (timer <= 0)
{
timer = pollRate;
PointMsg point = new PointMsg(1, 2, 3);
QuaternionMsg quat = new QuaternionMsg(1, 2, 3, 4);
PoseMsg pose = new PoseMsg(point, quat);
PoseWithCovarianceMsg posec = new PoseWithCovarianceMsg(pose);
Vector3Msg vec3 = new Vector3Msg(1, 2, 3);
TwistMsg twist = new TwistMsg(vec3, vec3);
TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
HeaderMsg header = new HeaderMsg(1, new TimeMsg(1, 1), "0");
PoseStampedMsg ps = new PoseStampedMsg(header, pose);
PathMsg msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });
BoolMsg boolmsg = new BoolMsg(true);
StringMsg str = new StringMsg("This is a test");
RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 1, 2, 3, true);
CameraInfoMsg caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);
_genericPub.PublishData(caminfo);
}
}
public static Quaternion From(this QuaternionMsg self, CoordinateSpaceSelection selection)
{
switch (selection)
{
case CoordinateSpaceSelection.RUF:
return(self.From <RUF>());
case CoordinateSpaceSelection.FLU:
return(self.From <FLU>());
case CoordinateSpaceSelection.FRD:
return(self.From <FRD>());
case CoordinateSpaceSelection.ENU:
return(self.From <ENU>());
case CoordinateSpaceSelection.NED:
return(self.From <NED>());
case CoordinateSpaceSelection.ENULocal:
return(self.From <ENULocal>());
case CoordinateSpaceSelection.NEDLocal:
return(self.From <NEDLocal>());
default:
Debug.LogError("Invalid coordinate space " + selection);
return(self.From <RUF>());
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
OccupancyGridMsg occupancy = (OccupancyGridMsg)msg;
MapMetaDataInfoMsg meta = occupancy.GetInfo();
int width = meta.GetWidth(), height = meta.GetHeight();
float resolution = meta.GetResolution();
PointMsg origin = meta.GetOrigin().GetPosition();
QuaternionMsg pose = meta.GetOrigin().GetOrientation();
Debug.Log("Width: " + width + "\nHeight: " + height + "\nResolution: " + resolution);
RawImage texObj = GameObject.Find(objectName).GetComponent <RawImage>();
Texture2D tex = texObj.texture as Texture2D;
if (tex == null || tex.width != width || tex.height != height)
{
tex = new Texture2D(width, height, TextureFormat.RGBA32, false);
texObj.texture = tex;
}
NativeArray <Color32> pixels = tex.GetRawTextureData <Color32>();
byte[] raw = occupancy.GetData();
for (int i = 0; i < raw.Length; i++)
{
pixels[i] = new Color32((byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)(255 - raw[i]), (byte)Mathf.Min(255, 350 - raw[i]));
}
tex.Apply();
}
public static Quaternion ToQuaternion(this QuaternionMsg quaternionMsg)
{
return(new Quaternion(
quaternionMsg.GetX(),
quaternionMsg.GetZ(),
quaternionMsg.GetY(),
quaternionMsg.GetW()));
}
public static Quaternion ToQuaternion(this QuaternionMsg quaternionMsg)
{
return(new Quaternion(
-quaternionMsg.GetY(),
quaternionMsg.GetZ(),
quaternionMsg.GetX(),
-quaternionMsg.GetW())); // negating because ROS is right handed and Unity is left handed
}
public PoseMsg(Transform tf)
{
Vector3 position = tf.position;
Vector3 orientation = tf.rotation.eulerAngles;
_position = new PointMsg(position.x, position.z, position.y);
_orientation = new QuaternionMsg(orientation.x, orientation.y, orientation.z);
}
public static void GUI(this QuaternionMsg message, string label)
{
if (label != "" && label != null)
{
label += ": ";
}
GUILayout.Label($"{label}[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]");
}
public ImuMessage(HeaderMsg header, QuaternionMsg quaternionIMU, double[] orientation_c, Vector3Msg angularVelocity, double[] angular_velocity_c, Vector3Msg linearAcceleration, double[] linear_acceleration_c)
{
_header = header;
_quaternionIMU = quaternionIMU;
_orientation_c = orientation_c;
_angularVelocity = angularVelocity;
_angular_velocity_c = angular_velocity_c;
_linearAcceleration = linearAcceleration;
_linear_acceleration_c = linear_acceleration_c;
}
public void sendPingerPos()
{
Vector3 posTmp = pinger.transform.position;
Quaternion rotTmp = pinger.transform.rotation;
pingerPoint = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
pingerQuaternion = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
pingerMsg = new PoseMsg(pingerPoint, pingerQuaternion);
obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PingerPublisher.GetMessageTopic(), pingerMsg);
}
public void SendPos(int id)
{
if (id != 0 && id <= props.Length)
{
Vector3 posTmp = props[id - 1].transform.position;
Quaternion rotTmp = props[id - 1].transform.rotation;
pt = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
qt = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
posMsg = new PoseMsg(pt, qt);
obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PosPublisher.GetMessageTopic(), posMsg);
}
}
public ImuMessage(JSONNode msg)
{
_header = new HeaderMsg(msg ["header"]);
_quaternionIMU = new QuaternionMsg(msg ["orientation"]);
/*
* for (int i = 0; i < 8; i++) {
* _orientation_c [i] = double.Parse (msg ["orientation_covariance"] [i]);
* _angular_velocity_c [i]= double.Parse (msg ["angular_velocity_covariance"][i]);
* _linear_acceleration_c [i]= double.Parse (msg ["linear_acceleration_covariance"][i]);
* }
*/
_angularVelocity = new Vector3Msg(msg ["angular_velocity"]);
_linearAcceleration = new Vector3Msg(msg ["linear_acceleration"]);
//Debug.Log ("IMU Message Done");
}
public PoseMsg(PointMsg _translation, QuaternionMsg _rotation)
{
position = _translation;
orientation = _rotation;
}
public PoseMsg(JSONNode msg)
{
position = new PointMsg(msg["position"]);
orientation = new QuaternionMsg(msg["orientation"]);
}
public TransformMsg(Transform tf)
{
_translation = new Vector3Msg(tf.localPosition, true);
_rotation = new QuaternionMsg(tf.localRotation, true);
}
public PoseMsg(Vector3 position, Quaternion orientation)
{
_position = new PointMsg(position);
_orientation = new QuaternionMsg(orientation);
}
public PoseMsg(PointMsg position, QuaternionMsg orientation)
{
_position = position;
_orientation = orientation;
}
public static Quaternion From <C>(this QuaternionMsg self) where C : ICoordinateSpace, new()
{
return(new Quaternion <C>((float)self.x, (float)self.y, (float)self.z, (float)self.w).toUnity);
}
public PoseMsg(Transform _tf)
{
this.position = new PointMsg(_tf.position, true);
this.orientation = new QuaternionMsg(_tf.localRotation, true);
}
public TransformMsg(JSONNode msg)
{
_translation = new Vector3Msg(msg["translation"]);
_rotation = new QuaternionMsg(msg["rotation"]);
}
public static void GUI(this QuaternionMsg message)
{
GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}, {message.w:F2}]");
}
public PoseMsg(PointMsg translation, QuaternionMsg rotation)
{
_position = translation;
_orientation = rotation;
}
public static void DrawAxisVectors <C>(Drawing3d drawing, Vector3Msg position, QuaternionMsg rotation, float size, bool drawUnityAxes) where C : ICoordinateSpace, new()
{
Vector3 unityPosition = position.From <C>();
Quaternion unityRotation = rotation.From <C>();
Vector3 x, y, z;
if (drawUnityAxes)
{
x = unityRotation * new Vector3(1, 0, 0) * size;
y = unityRotation * new Vector3(0, 1, 0) * size;
z = unityRotation * new Vector3(0, 0, 1) * size;
}
else
{
x = unityRotation * new Vector3 <C>(1, 0, 0).toUnity *size;
y = unityRotation * new Vector3 <C>(0, 1, 0).toUnity *size;
z = unityRotation * new Vector3 <C>(0, 0, 1).toUnity *size;
}
drawing.DrawLine(unityPosition, unityPosition + x, Color.red, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + y, Color.green, size * 0.1f);
drawing.DrawLine(unityPosition, unityPosition + z, Color.blue, size * 0.1f);
}
public override void Deserialize(JSONNode msg)
{
_position = new PointMsg(msg["position"]);
_orientation = new QuaternionMsg(msg["orientation"]);
}
public PoseMsg(PointMsg p, QuaternionMsg q)
{
_position = p;
_orientation = q;
}
public PoseMsg(Vector3 _position, Quaternion _orientation, bool _fromUnity = false)
{
position = new PointMsg(_position, _fromUnity);
orientation = new QuaternionMsg(_orientation, _fromUnity);
}
public TransformMsg(PointMsg p, QuaternionMsg q)
{
_translation = p;
_rotation = q;
}
public TransformMsg(Vector3Msg translation, QuaternionMsg rotation)
{
_translation = translation;
_rotation = rotation;
}
public ROSBridgeMsg OnReceiveMessage(string topic, JSONNode raw_msg, ROSBridgeMsg parsed = null)
{
ROSBridgeMsg result = null;
// Writing all code in here for now. May need to move out to separate handler functions when it gets too unwieldy.
switch (topic)
{
case "/dji_sdk/attitude":
QuaternionMsg attitudeMsg = (parsed == null) ? new QuaternionMsg(raw_msg["quaternion"]) : (QuaternionMsg)parsed;
attitude = offset * (new Quaternion(attitudeMsg.GetX(), attitudeMsg.GetY(), attitudeMsg.GetZ(), attitudeMsg.GetW()));
result = attitudeMsg;
break;
case "/dji_sdk/battery_state":
battery_state = (parsed == null) ? new BatteryStateMsg(raw_msg) : (BatteryStateMsg)parsed;
result = battery_state;
break;
case "/dji_sdk/flight_status":
if (isM100)
{
m100_flight_status = (FlightStatusM100)(new UInt8Msg(raw_msg)).GetData();
}
else
{
flight_status = (FlightStatus)(new UInt8Msg(raw_msg)).GetData();
}
break;
case "/dji_sdk/gimbal_angle":
Vector3Msg gimbleAngleMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
gimble_joint_angles = new Vector3((float)gimbleAngleMsg.GetX(), (float)gimbleAngleMsg.GetY(), (float)gimbleAngleMsg.GetZ());
result = gimbleAngleMsg;
break;
case "/dji_sdk/gps_health":
gps_health = (parsed == null) ? (new UInt8Msg(raw_msg)).GetData() : ((UInt8Msg)parsed).GetData();
break;
case "/dji_sdk/gps_position":
gps_position = (parsed == null) ? new NavSatFixMsg(raw_msg) : (NavSatFixMsg)parsed;
result = gps_position;
break;
case "/dji_sdk/imu":
imu = (parsed == null) ? new IMUMsg(raw_msg) : (IMUMsg)parsed;
result = imu;
break;
case "/dji_sdk/rc":
remote_controller_msg = (parsed == null) ? new JoyMsg(raw_msg) : (JoyMsg)parsed;
result = remote_controller_msg;
break;
case "/dji_sdk/velocity":
Vector3Msg velocityMsg = (parsed == null) ? new Vector3Msg(raw_msg["vector"]) : (Vector3Msg)parsed;
velocity = new Vector3((float)velocityMsg.GetX(), (float)velocityMsg.GetY(), (float)velocityMsg.GetZ());
result = velocityMsg;
break;
case "/dji_sdk/height_above_takeoff":
relative_altitude = (parsed == null) ? (new Float32Msg(raw_msg)).GetData() : ((Float32Msg)parsed).GetData();
break;
case "/dji_sdk/local_position":
PointMsg pointMsg = (parsed == null) ? new PointMsg(raw_msg["point"]) : (PointMsg)parsed;
local_position = new Vector3(pointMsg.GetX(), pointMsg.GetY(), pointMsg.GetZ());
result = pointMsg;
Debug.Log(result);
break;
default:
Debug.LogError("Topic not implemented: " + topic);
break;
}
return(result);
}
