private Vector3 getVectFromMessage(GeometryPose pose, Vector3 vec)
{
Vector3 scale = new Vector3(0.1f, 0.1f, 0.1f);
Vector3 rosPos = new Vector3(pose.position.x, pose.position.z, pose.position.y);
//rosPos.Scale(scale);
return(rosPos + vec);
}
public NavigationMapMetaData()
{
map_load_time = null;
resolution = 0;
width = 0;
height = 0;
origin = new GeometryPose();
}
GeometryPose UpdateMessageContents(GameObject TargetModel)
{
GeometryPose TargetPose = new GeometryPose();
Vector3 position = TargetModel.transform.position - UrdfModel.transform.position;
Quaternion rotation = UrdfModel.transform.rotation * TargetModel.transform.rotation;
TargetPose.position = GetGeometryPoint(position.Unity2Ros());
TargetPose.position = new GeometryPoint {
x = -TargetPose.position.x,
y = -TargetPose.position.y,
z = TargetPose.position.z
};
TargetPose.orientation = GetGeometryQuaternion(rotation.Unity2Ros());
return(TargetPose);
}
public Marker()
{
header = new StandardHeader();
ns = "";
id = 0;
type = 0;
action = 0;
pose = new GeometryPose();
scale = new GeometryVector3();
color = new ColorRGBA();
//duration lifetime ???
frame_locked = false;
points = new GeometryPoint[0];
colors = new ColorRGBA[0];
text = "";
mesh_resource = "";
mesh_use_embedded_materials = false;
}
public GeometryPoseStamped()
{
header = new StandardHeader();
pose = new GeometryPose();
}
public GeometryPoseWithCovariance()
{
pose = new GeometryPose();
covariance = new float[36];
}
private Quaternion getQuadFromMsg(GeometryPose pose)
{
return(new Quaternion(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w));
}