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 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);
}