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