public float GetInputAxisOrButtons(JoyMsg message, JoyInputType axis, JoyInputType upButton, JoyInputType downButton)
{
if (axis != JoyInputType.None)
{
return(GetInputFloat(message, axis));
}
return(GetInputFloat(message, upButton) - GetInputFloat(message, downButton));
}
public void PublishRateSetpoint(float roll, float pitch, float height, float yaw_rate)
{
int num_seconds = DateTime.UtcNow.Second - epoch.Second;
HeaderMsg header = new HeaderMsg(seq_no_rate++, new TimeMsg(num_seconds, (int)(DateTime.UtcNow.Ticks - ((long)num_seconds) * (10000000))), "");
float[] axes = new float[4];
axes[0] = Mathf.Deg2Rad * roll;
axes[1] = Mathf.Deg2Rad * pitch;
axes[2] = height;
axes[3] = yaw_rate;
JoyMsg msg = new JoyMsg(header, axes, dummy);
ros.Publish("/dji_sdk/flight_control_setpoint_rollpitch_yawrate_zposition", msg);
}
public void PublishRelativeSetPoint(Vector2 position_offset, float relative_altitude, float yaw_angle)
{
int num_seconds = DateTime.UtcNow.Second - epoch.Second;
HeaderMsg header = new HeaderMsg(seq_no_point++, new TimeMsg(num_seconds, (int)(DateTime.UtcNow.Ticks - ((long)num_seconds) * (10000000))), "");
float[] axes = new float[4];
axes[0] = position_offset.x;
axes[1] = position_offset.y;
axes[2] = relative_altitude;
axes[3] = yaw_angle;
JoyMsg msg = new JoyMsg(header, axes, dummy);
ros.Publish("/dji_sdk/flight_control_setpoint_ENUposition_yaw", msg);
}
public bool GetInputBool(JoyMsg message, JoyInputType inputType)
{
if (inputType >= JoyInputType.Button_0)
{
int buttonIndex = inputType - JoyInputType.Button_0;
if (buttonIndex < 0 || buttonIndex >= message.buttons.Length)
{
return(false);
}
return(message.buttons[buttonIndex] != 0);
}
int axisIndex = inputType - JoyInputType.Axis_0;
if (axisIndex < 0 || axisIndex >= message.axes.Length)
{
return(false);
}
return(message.axes[axisIndex] != 0);
}
public float GetInputFloat(JoyMsg message, JoyInputType inputType)
{
if (inputType >= JoyInputType.Button_0)
{
int buttonIndex = inputType - JoyInputType.Button_0;
if (buttonIndex < 0 || buttonIndex >= message.buttons.Length)
{
return(0.0f);
}
return(message.buttons[buttonIndex] != 0 ? 1.0f : 0.0f);
}
int axisIndex = inputType - JoyInputType.Axis_0;
if (axisIndex < 0 || axisIndex >= message.axes.Length)
{
return(0.0f);
}
return(message.axes[axisIndex]);
}
public Action CreateGUI(JoyMsg message, MessageMetadata meta)
{
return(() =>
{
message.header.GUI();
Rect layout = GUILayoutUtility.GetAspectRect(2);
float minX = layout.xMin;
float maxX = layout.xMax;
float midX = (minX + maxX) / 2;
float minY = layout.yMin;
float maxY = layout.yMax;
float midY = (minY + maxY) / 2;
float w = layout.width;
float h = layout.height;
float sw = w * 0.125f;
float sh = h * 0.25f;
float bw = w * 0.0625f;
float bh = h * 0.125f;
// Triggers
GUI.Box(new Rect(minX + sw, minY, bw, sh), TriggerTexture(GetInputFloat(message, m_LTrigger)));
GUI.Box(new Rect(maxX - sw * 1.5f, minY, bw, sh), TriggerTexture(GetInputFloat(message, m_RTrigger)));
// Shoulders
GUI.Box(new Rect(minX + sw * 1.5f, minY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_LShoulder)));
GUI.Box(new Rect(maxX - sw * 2.0f, minY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_RShoulder)));
// Dpad, central buttons
GUI.Box(new Rect(midX - sw * 2.5f, maxY - sh, sw, sh),
StickTexture(
GetInputAxisOrButtons(message, m_DPad_XAxis, m_DPad_Left, m_DPad_Right),
GetInputAxisOrButtons(message, m_DPad_YAxis, m_DPad_Up, m_DPad_Down),
false)
);
if (m_Button_Back != JoyInputType.None)
{
GUI.Box(new Rect(midX - bw * 2.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Back)));
}
if (m_Button_Power != JoyInputType.None)
{
GUI.Box(new Rect(midX - bw * 0.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Power)));
}
if (m_Button_Start != JoyInputType.None)
{
GUI.Box(new Rect(midX + bw * 1.5f, midY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_Start)));
}
// N/E/S/W buttons
float btnX = maxX - sw * 1.25f;
float btnY = midY - bh * 0.5f;
GUI.Box(new Rect(btnX - bw, btnY, bw, bh), ButtonTexture(GetInputBool(message, m_Button_West)));
GUI.Box(new Rect(btnX, btnY - bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_North)));
GUI.Box(new Rect(btnX, btnY + bh, bw, bh), ButtonTexture(GetInputBool(message, m_Button_South)));
GUI.Box(new Rect(btnX + bw, btnY, bw, bh), ButtonTexture(GetInputBool(message, m_Button_East)));
// Joysticks
GUI.Box(new Rect(minX + sw * 0.5f, midY - sh * 0.5f, sw, sh), StickTexture(GetInputFloat(message, m_LStick_X), GetInputFloat(message, m_LStick_Y), GetInputBool(message, m_LStick_Click)));
GUI.Box(new Rect(midX + sw * 1.5f, maxY - sh, sw, sh), StickTexture(GetInputFloat(message, m_RStick_X), GetInputFloat(message, m_RStick_Y), GetInputBool(message, m_RStick_Click)));
});
}
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);
}