public override void HandleEvent(QMsg msg)
{
PointMsg pm = msg as PointMsg;
switch (pm.controllmode)
{
case PointControll_E.Edit:
break;
case PointControll_E.SaveMatchingpoint:
break;
case PointControll_E.Cancle:
break;
case PointControll_E.AutoNext:
break;
case PointControll_E.Load:
LoadPointer(pm.parent, pm.pointsmap);
break;
case PointControll_E.ChoiseGroup:
case PointControll_E.AllDisplay:
RefreshDisplay(pm.controllmode, pm);
break;
default:
break;
}
}
//点控制
void ControllPoint(QMsg msg)
{
PointMsg pm = msg as PointMsg;
PointControll_E pce = pm.controllmode;
currentmode = pce;
switch (pce)
{
case PointControll_E.Edit:
CheckLastEdit(currentgroup, currentindex);
this.currentgroup = pm.group;
this.currentindex = pm.index;
break;
case PointControll_E.SaveMatchingpoint:
SaveGizmo();
break;
case PointControll_E.Cancle:
CancleGizmo();
break;
case PointControll_E.AutoNext:
break;
default:
break;
}
}
public void InitCameraManager()
{
GameObject camera = new GameObject("GameCamera", typeof(Camera));
camerarotatesphere = camera.AddComponent <CameraRotateSphere>();
camera.transform.SetParent(transform);
QMsg Normalmsg = new ModelMsg()
{
EventID = (int)Model_E.Normal, modelevent = Model_E.SetCamera, selfcamera = camera.GetComponent <Camera>()
};
SendMsg(Normalmsg);
QMsg Usermsg = new ModelMsg()
{
EventID = (int)Model_E.UserImport, modelevent = Model_E.SetCamera, selfcamera = camera.GetComponent <Camera>()
};
SendMsg(Usermsg);
QMsg normalPointmsg = new PointMsg()
{
EventID = (int)Point_E.Normal, controllmode = PointControll_E.SetCamra, selfcamra = camera.GetComponent <Camera>()
};
SendMsg(normalPointmsg);
QMsg userPointmsg = new PointMsg()
{
EventID = (int)Point_E.Userimport, controllmode = PointControll_E.SetCamra, selfcamra = camera.GetComponent <Camera>()
};
SendMsg(userPointmsg);
RegetsSelfEvent();
}
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 Vector3 From(this PointMsg 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>());
}
}
void ResetLastGroupItem(Button currentlistbutton, int group)
{
if (lastkey)
{
lastkey.GetComponent <Image>().color = Color.white;
}
currentlistbutton.GetComponent <Image>().color = Color.green;
//判断当前是分组显示
if (dropdownindex == 0)
{
QMsg usermsg = new PointMsg()
{
EventID = (int)Point_E.Userimport,
group = group,
controllmode = PointControll_E.ChoiseGroup
};
PointerManager.Instance.SendMsg(usermsg);
QMsg normalmsg = new PointMsg()
{
EventID = (int)Point_E.Normal,
group = group,
controllmode = PointControll_E.ChoiseGroup
};
PointerManager.Instance.SendMsg(normalmsg);
}
currentgroup = group;
// lastkey = currentlistbutton;
}
public static Vector3 ToVector3(this PointMsg pointMsg)
{
return(new Vector3(
-pointMsg.GetY(),
pointMsg.GetZ(),
pointMsg.GetX()));
}
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 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 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 static void GUI(this PointMsg message, string name)
{
string body = $"[{message.x:F2}, {message.y:F2}, {message.z:F2}]";
if (name == null || name == "")
{
GUILayout.Label(body);
}
else
{
GUILayout.Label($"{name}: {body}");
}
}
public DetectionMsg(JSONNode msg)
{
scores = new ObjectHypothesisMsg[msg["scores"].Count];
for (int i = 0; i < scores.Length; i++)
{
scores[i] = new ObjectHypothesisMsg(msg["scores"][i]);
}
corners = new PointMsg[msg["corners"].Count];
for (int i = 0; i < corners.Length; i++)
{
corners[i] = new PointMsg(msg["corners"][i]);
}
occluded_corners = byte.Parse(msg["occluded_corners"], System.Globalization.CultureInfo.InvariantCulture);
}
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"]);
}
//封装发送
private void Send(PointControll_E pcontroller)
{
QMsg normalmsg = new PointMsg()
{
EventID = (int)Point_E.Normal, controllmode = pcontroller, group = this.group, index = this.index
};
PointerManager.Instance.SendMsg(normalmsg);
QMsg usermsg = new PointMsg()
{
EventID = (int)Point_E.Userimport, controllmode = pcontroller, group = this.group, index = this.index
};
PointerManager.Instance.SendMsg(usermsg);
}
//封装发送
private void Send(PointControll_E pcontroller)
{
QMsg normalmsg = new PointMsg()
{
EventID = (int)Point_E.Normal, controllmode = pcontroller
};
PointerManager.Instance.SendMsg(normalmsg);
QMsg usermsg = new PointMsg()
{
EventID = (int)Point_E.Userimport, controllmode = pcontroller
};
PointerManager.Instance.SendMsg(usermsg);
lastchoiseitem.HandleEvent(pcontroller);
toolpanel.SetActive(pcontroller == PointControll_E.Edit);
}
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());
}
protected virtual void RefreshDisplay(PointControll_E pce, QMsg msg)
{
PointMsg pm = msg as PointMsg;
switch (pce)
{
case PointControll_E.ChoiseGroup:
Refreshdis(pm.group);
break;
case PointControll_E.AllDisplay:
Refreshdis(allactive: true);
break;
default:
break;
}
}
/// <summary>
/// ("分组显示", "全部显示")下拉列表，事件定义
/// </summary>
/// <param name="index"></param>
void DropdownCallBack(int index)
{
dropdownindex = index;
QMsg usermsg = new PointMsg()
{
EventID = (int)Point_E.Userimport,
group = currentgroup,
controllmode = index == 0 ? PointControll_E.ChoiseGroup : PointControll_E.AllDisplay
};
PointerManager.Instance.SendMsg(usermsg);
QMsg normalmsg = new PointMsg()
{
EventID = (int)Point_E.Normal,
group = currentgroup,
controllmode = index == 0 ? PointControll_E.ChoiseGroup : PointControll_E.AllDisplay
};
PointerManager.Instance.SendMsg(normalmsg);
//Enums.PointControll pcontroll = (Enums.PointControll)index;
//MSGCenter.Execute(pcontroll.ToString(),choiseindex.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;
}
/// <summary>
/// 加载用户点
/// </summary>
protected virtual void LoadGizmopointer(Dictionary <int, Dictionary <int, Vector3> > _pointmap, Transform _parent, Tool.PointMode mode)
{
int eventid = -1;
switch (mode)
{
case Tool.PointMode.Normal:
eventid = (int)Point_E.Normal;
break;
case Tool.PointMode.UserImport:
eventid = (int)Point_E.Userimport;
break;
default:
break;
}
QMsg msg = new PointMsg()
{
EventID = eventid, controllmode = PointControll_E.Load, parent = _parent, pointsmap = _pointmap
};
PointerManager.Instance.SendMsg(msg);
}
public PoseMsg(Vector3 _position, Quaternion _orientation, bool _fromUnity = false)
{
position = new PointMsg(_position, _fromUnity);
orientation = new QuaternionMsg(_orientation, _fromUnity);
}
public PoseMsg(Transform _tf)
{
this.position = new PointMsg(_tf.position, true);
this.orientation = new QuaternionMsg(_tf.localRotation, true);
}
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(PointMsg p, QuaternionMsg q)
{
_translation = p;
_rotation = q;
}
public TransformMsg(JSONNode msg)
{
_translation = new PointMsg(msg["translation"]);
_rotation = new QuaternionMsg(msg["rotation"]);
}
public override void Deserialize(JSONNode msg)
{
_position = new PointMsg(msg["position"]);
_orientation = new QuaternionMsg(msg["orientation"]);
}
public static void GUI(this PointMsg message)
{
GUILayout.Label($"[{message.x:F2}, {message.y:F2}, {message.z:F2}]");
}
/// <summary>
/// Finds the world coordinates of an image coordinate based on the CameraInfo matrices
/// K matrix P matrix
/// 0 1 2 0 1 2 3
/// 3 4 5 4 5 6 7
/// 6 7 8 8 9 10 11
/// </summary>
/// <param name="x">x coord to map</param>
/// <param name="y">y coord to map</param>
/// <param name="P">Projection matrix (3x4)</param>
/// <param name="K">Camera intrinsics (3x3)</param>
/// <returns></returns>
static PointMsg FindPixelProjection(int x, int y, double[] P, double[][] invK)
{
PointMsg worldCoord = new PointMsg(x, y, 1);
return(new PointMsg((float)invK[0][0] * worldCoord.x + (float)invK[0][1] * worldCoord.y + (float)invK[0][2] * worldCoord.z, (float)invK[1][0] * worldCoord.x + (float)invK[1][1] * worldCoord.y + (float)invK[1][2] * worldCoord.z, (float)invK[2][0] * worldCoord.x + (float)invK[2][1] * worldCoord.y + (float)invK[2][2] * worldCoord.z));
}
