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 CameraInfoMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_height = uint.Parse(msg["height"]);
_distortion_model = msg["distortion_model"];
_d = new double[msg["D"].Count];
_k = new double[msg["K"].Count];
_r = new double[msg["R"].Count];
_p = new double[msg["P"].Count];
for (int i = 0; i < msg["D"].Count; i++)
{
_d[i] = double.Parse(msg["D"][i], CultureInfo.InvariantCulture);
}
for (int i = 0; i < msg["K"].Count; i++)
{
_k[i] = double.Parse(msg["K"][i], CultureInfo.InvariantCulture);
}
for (int i = 0; i < msg["R"].Count; i++)
{
_r[i] = double.Parse(msg["R"][i], CultureInfo.InvariantCulture);
}
for (int i = 0; i < msg["P"].Count; i++)
{
_p[i] = double.Parse(msg["P"][i], CultureInfo.InvariantCulture);
}
_binning_x = uint.Parse(msg["binning_x"], CultureInfo.InvariantCulture);
_binning_y = uint.Parse(msg["binning_y"], CultureInfo.InvariantCulture);
_roi = new RegionOfInterestMsg(msg["roi"]);
}
public static void GUI(this RegionOfInterestMsg message, Texture2D tex = null)
{
if (tex != null)
{
var ratio = (float)tex.width / (float)tex.height;
UnityEngine.GUI.Box(GUILayoutUtility.GetAspectRect(ratio), tex);
}
GUILayout.Label($"x_offset: {message.x_offset}\ny_offset: {message.y_offset}\nHeight: {message.height}\nWidth: {message.width}\nDo rectify: {message.do_rectify}");
}
public CameraInfoMsg(HeaderMsg header, uint height, uint width, string distortion_model, double[] d, double[] k, double[] r, double[] p, uint binning_x, uint binning_y, RegionOfInterestMsg roi)
{
_header = header;
_height = height;
_width = width;
_distortion_model = distortion_model;
_d = d;
_k = k;
_r = r;
_p = p;
_binning_x = binning_x;
_binning_y = binning_y;
_roi = roi;
}
protected override void PublishSensorData()
{
_camera.targetTexture = _renderTexture;
_camera.Render();
RenderTexture.active = _renderTexture;
_texture2D.ReadPixels(_rect, 0, 0);
_camera.targetTexture = null;
RenderTexture.active = null;
HeaderMsg header = new HeaderMsg(_sequenceId, new TimeMsg(0, 0), "raspicam");
RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 0, 0, 0, false);
CameraInfoMsg camInfo = new CameraInfoMsg(header, (uint)_resolutionHeight, (uint)_resolutionWidth, "plumb_bob", Distortion, CameraMatrix, Rectification, Projection, 0, 0, roi);
_cameraSensorPublisher.PublishData(_texture2D, _cameraQualityLevel, _sequenceId);
_cameraInfoPublisher.PublishData(camInfo);
_sequenceId++;
}
/// <summary>
/// Creates a new Texture2D that grabs a region of interest of a given texture, if applicable. Otherwise, an approximated empty texture with a highlighted region is returned.
/// </summary>
public static Texture2D RegionOfInterestTexture(this RegionOfInterestMsg message, Texture2D rawTex, int height = 0, int width = 0)
{
int mWidth = (int)message.width;
int mHeight = (int)message.height;
int x_off = ((int)message.x_offset == mWidth) ? 0 : (int)message.x_offset;
int y_off = ((int)message.y_offset == mHeight) ? 0 : (int)message.y_offset;
Texture2D overlay;
if (rawTex == null)
{
// No texture provided, just return approximation
if (width == 0 || height == 0)
{
overlay = new Texture2D(x_off + mWidth + 10, y_off + mHeight + 10);
}
else
{
overlay = new Texture2D(width, height);
}
// Initialize ROI color block
Color[] colors = new Color[mHeight * mWidth];
for (int i = 0; i < colors.Length; i++)
{
colors[i] = Color.red;
}
overlay.SetPixels(x_off, y_off, mWidth, mHeight, colors);
}
else
{
// Crop out ROI from input texture
overlay = new Texture2D(mWidth - x_off, mHeight - y_off, rawTex.format, true);
overlay.SetPixels(0, 0, overlay.width, overlay.height, rawTex.GetPixels(x_off, 0, mWidth - x_off, mHeight - y_off));
}
overlay.Apply();
return(overlay);
}
