public void Visualize()
{
if (!isCreated)
{
Create();
}
markerObject.name = name;
markerObject.transform.localPosition = TransformExtensions.Ros2Unity(TypeExtensions.PointMsgToVector3(marker.pose.position));
markerObject.transform.localRotation = TransformExtensions.Ros2Unity(TypeExtensions.QuaternionMsgToQuaternion(marker.pose.orientation));
markerObject.transform.localScale = TransformExtensions.Ros2UnityScale(TypeExtensions.Vector3MsgToVector3(marker.scale));
if (marker.type == MessageTypes.Visualization.Marker.CYLINDER)
{
markerObject.transform.localScale = new Vector3(markerObject.transform.localScale.x, markerObject.transform.localScale.y / 2, markerObject.transform.localScale.z);
}
markerObject.GetComponent <Renderer>().material.SetColor("_Color", TypeExtensions.ColorRGBAToColor(marker.color));
if (!marker.frame_locked)
{
markerObject.transform.parent = null;
}
}
void UpdateDroneOdom(Odometry odom)
{
Vector3 pos = odom.pose.pose.position.ToUnity();
Quaternion quat = new Quaternion((float)odom.pose.pose.orientation.x, (float)odom.pose.pose.orientation.y, (float)odom.pose.pose.orientation.z, (float)odom.pose.pose.orientation.w);
Quaternion rot = TransformExtensions.Ros2Unity(quat);
Drone.transform.localPosition = pos;
Drone.transform.localRotation = rot;
}
public static UnityEngine.Vector3 ToUnity(this Point p, bool ros2Unity = true)
{
UnityEngine.Vector3 v = new UnityEngine.Vector3();
v.x = (float)p.x;
v.y = (float)p.y;
v.z = (float)p.z;
if (ros2Unity)
{
return(TransformExtensions.Ros2Unity(v));
}
else
{
return(v);
}
}
