public new static void CallBack(ROSBridgeMsg msg)
{
GameObject model = GameObject.Find("Model");
GameObject robot = GameObject.Find("Robot");
Robot r = (Robot)model.GetComponent(typeof(Robot));
if (model == null)
{
Debug.Log("<color=red>ERROR:</color> Can't find GameObject Robot. Create it in Unity");
}
else
{
NavStsMsg nav_sts = (NavStsMsg)msg;
NEDMsg p = nav_sts.GetPosition();
RPYMsg o = nav_sts.GetOrientation();
robot.transform.position = new Vector3(-p.GetNorth(), 0, p.GetEast());
Vector3 next_position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
Quaternion next_orientation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
r.AddWaypoint(next_position, next_orientation);
//model.transform.position = new Vector3(-p.GetNorth(), -p.GetDepth(), p.GetEast());
//model.transform.rotation = Quaternion.Euler(o.GetRollDegrees() + 90f, o.GetYawDegrees(), o.GetPitchDegrees());
GameObject ned_canvas = GameObject.Find("NED");
Text NED_text = ned_canvas.GetComponent <Text>();
int dp = 2;
string format = string.Format("#.{0} m;-#.{0} m", new string('#', dp));
NED_text.text = p.GetNorth().ToString(format) +
"\n" + p.GetEast().ToString(format) +
"\n" + p.GetDepth().ToString(format) +
"\n" + nav_sts.GetAltitude().GetData().ToString(format);
}
}
public NavStsMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_global_position = new DecimalLatLonMsg(msg["global_position"]);
_origin = new DecimalLatLonMsg(msg["origin"]);
_position = new NEDMsg(msg["position"]);
_altitude = new Float32Msg(msg["altitude"]);
_body_velocity = new PointMsg(msg["body_velocity"]);
_orientation = new RPYMsg(msg["orientation"]);
_orientation_rate = new RPYMsg(msg["orientation_rate"]);
_position_variance = new NEDMsg(msg["position_variance"]);
_orientation_variance = new RPYMsg(msg["orientation_variance"]);
_status = new UInt8Msg(msg["status"]);
}
public NavStsMsg(JSONNode msg)
{
//Debug.Log("NavStsMsg with " + msg.ToString());
_header = new HeaderMsg(msg["header"]);
_global_position = new DecimalLatLonMsg(msg["global_position"]);
_origin = new DecimalLatLonMsg(msg["origin"]);
_position = new NEDMsg(msg["position"]);
_altitude = new Float32Msg(msg["altitude"]);
_body_velocity = new PointMsg(msg["body_velocity"]);
_orientation = new RPYMsg(msg["orientation"]);
_orientation_rate = new RPYMsg(msg["orientation_rate"]);
_position_variance = new NEDMsg(msg["position_variance"]);
_orientation_variance = new RPYMsg(msg["orientation_variance"]);
_status = new UInt8Msg(msg["status"]);
//Debug.Log("NavStsMsg done and it looks like " + this.ToString());
}
public NavStsMsg(HeaderMsg header,
DecimalLatLonMsg global_position,
DecimalLatLonMsg origin,
NEDMsg position,
Float32Msg altitude,
PointMsg body_velocity,
RPYMsg orientation,
RPYMsg orientation_rate,
NEDMsg position_variance,
RPYMsg orientation_variance,
UInt8Msg status)
{
_header = header;
_global_position = global_position;
_origin = origin;
_position = position;
_altitude = altitude;
_body_velocity = body_velocity;
_orientation = orientation;
_orientation_rate = orientation_rate;
_position_variance = position_variance;
_orientation_variance = orientation_variance;
_status = status;
}
