IEnumerator SendImu()
{
yield return(new WaitForEndOfFrame());
var now = DateTime.Now;
var timeSpan = now - lastFrame;
var timeSinceStart = now - camStart;
var timeMessage = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
var headerMessage = new HeaderMsg(count, timeMessage, "imu");
Quaternion vehicleImu;
vehicleImu = vehicle.transform.rotation;
double xey = vehicleImu.x;
double yey = vehicleImu.y;
double zey = vehicleImu.z;
double wey = vehicleImu.w;
var imuData = new QuaternionMsg(xey, yey, zey, wey);
double[] arg3 = { 0.00, 0.00, 0.00 };
var vect3 = new Vector3Msg(0.00, 0.00, 0.00);
var imumessage = new ImuMessage(headerMessage, imuData, arg3, vect3, arg3, vect3, arg3);
ros.Publish(imuPublisher.GetMessageTopic(), imumessage);
ros.Render();
}
public void PublishData(Texture2D texture2D, int qualityLevel, int sequenceId)
{
HeaderMsg header = new HeaderMsg(sequenceId, new TimeMsg(0, 0), "raspicam");
CompressedImageMsg imageMsg = new CompressedImageMsg(header, "jpg", texture2D.EncodeToJPG(qualityLevel));
_publisher.PublishData(imageMsg);
}
public OdometryMsg(PoseWithCovarianceMsg pose)
{
_header = new HeaderMsg(0, new TimeMsg(0, 0), "0");
_child_frame_id = "0";
_pose = pose;
_twist = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0)), new double[36]);
}
public CompressedImageMsg(HeaderMsg header, string format,
byte[] data)
{
_header = header;
_format = format;
_data = data;
}
IEnumerator SendImage()
{
yield return(new WaitForEndOfFrame());
int width = 640;
int height = 480;
Texture2D tex = new Texture2D(width, height, TextureFormat.RGB24, false);
tex.ReadPixels(new Rect(0, 0, width, height), 0, 0);
tex.Apply();
byte[] data = tex.EncodeToJPG();
var now = DateTime.Now;
var timeSpan = now - lastFrame;
var timeSinceStart = now - camStart;
var timeMessage = new TimeMsg(timeSinceStart.Seconds, timeSinceStart.Milliseconds);
var headerMessage = new HeaderMsg(count, timeMessage, "camera");
Debug.Log("data length: " + data.Length);
string format = "jpeg";
var compressedImageMsg = new CompressedImageMsg(headerMessage, format, data);
Debug.Log(compressedImageMsg);
ros.Publish(CompressedImagePublisher.GetMessageTopic(), compressedImageMsg);
Destroy(tex);
ros.Render();
}
public RNDataMsg(JSONNode node)
{
header = new HeaderMsg(node["header"]);
width = node["width"].AsInt;
is_bigendian = node["is_bigendian"].AsBool;
point_step = node["point_step"].AsInt;
scalar_step = node["scalar_step"].AsInt;
is_dense = node["is_dense"].AsBool;
JSONArray fields_arr = node["fields"].AsArray;
fields = new RNPointFieldMsg[fields_arr.Count];
for (int i = 0; i < fields.Length; i++)
{
fields[i] = new RNPointFieldMsg(fields_arr[i]);
}
fields_arr = node["scalar_fields"].AsArray;
scalar_fields = new RNPointFieldMsg[fields_arr.Count];
for (int i = 0; i < scalar_fields.Length; i++)
{
scalar_fields[i] = new RNPointFieldMsg(fields_arr[i]);
}
data = ROSBridgeUtils.ParseJSONRawData(node["data"], is_bigendian);
}
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 OdometryMsg(HeaderMsg header, string child_frame_id, PoseWithCovarianceMsg pose, TwistWithCovarianceMsg twist)
{
_header = header;
_child_frame_id = child_frame_id;
_pose = pose;
_twist = twist;
}
public void SetTFTrackingSettings(TFTrackingSettings tfTrackingType, HeaderMsg headerMsg)
{
switch (tfTrackingType.type)
{
case TFTrackingType.Exact:
{
TFFrame frame = TFSystem.instance.GetTransform(headerMsg, tfTrackingType.tfTopic);
transform.position = frame.translation;
transform.rotation = frame.rotation;
}
break;
case TFTrackingType.TrackLatest:
{
transform.parent = TFSystem.instance.GetTransformObject(headerMsg.frame_id, tfTrackingType.tfTopic).transform;
transform.localPosition = Vector3.zero;
transform.localRotation = Quaternion.identity;
}
break;
case TFTrackingType.None:
transform.localPosition = Vector3.zero;
transform.localRotation = Quaternion.identity;
break;
}
}
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 OdometryMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_child_frame_id = msg["child_frame_id"];
_pose = new PoseWithCovarianceMsg(msg["pose"]);
_twist = new TwistWithCovarianceMsg(msg["twist"]);
}
private IEnumerator SendPathToROS(ROS ros)
{
while (Application.isPlaying)
{
if (ros.IsConnected())
{
Vector3[] points = agent.path.corners;
PoseStampedMsg[] poses = new PoseStampedMsg[points.Length];
HeaderMsg head = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
Quaternion rotation = transform.rotation;
for (int i = 0; i < points.Length; i++)
{
head.SetSeq(i);
if (i > 0)
{
rotation = Quaternion.FromToRotation(points[i - 1], points[i]);
}
poses[i] = new PoseStampedMsg(head, new PoseMsg(points[i], rotation, true));
}
HeaderMsg globalHead = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
PathMsg pathmsg = new PathMsg(globalHead, poses);
ros.Publish(Path_pub.GetMessageTopic(), pathmsg);
}
yield return(new WaitForSeconds(rOSFrecuencyPath));
}
}
public override void Deserialize(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_child_frame_id = msg["child_frame_id"].ToString();
_pose = new PoseWithCovarianceMsg(msg["pose"]);
_twist = new TwistWithCovarianceMsg(msg["twist"]);
}
public BatteryStateMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
if (!msg["capacity"].Value.Equals("null"))
{
_capacity = float.Parse(msg["capacity"], System.Globalization.CultureInfo.InvariantCulture);
}
if (!msg["charge"].Value.Equals("null"))
{
_charge = float.Parse(msg["charge"], System.Globalization.CultureInfo.InvariantCulture);
}
_current = float.Parse(msg["current"], System.Globalization.CultureInfo.InvariantCulture);
_design_capacity = float.Parse(msg["design_capacity"], System.Globalization.CultureInfo.InvariantCulture);
if (!msg["percentage"].Value.Equals("null"))
{
_percentage = float.Parse(msg["percentage"], System.Globalization.CultureInfo.InvariantCulture);
}
_voltage = float.Parse(msg["voltage"], System.Globalization.CultureInfo.InvariantCulture);
_cell_voltage = new float[msg["cell_voltage"].Count];
for (int i = 0; i < _cell_voltage.Length; i++)
{
_cell_voltage[i] = float.Parse(msg["cell_voltage"][i], System.Globalization.CultureInfo.InvariantCulture);
}
_location = msg["location"];
_power_supply_health = uint.Parse(msg["power_supply_health"]);
_power_supply_status = uint.Parse(msg["power_supply_status"]);
_power_supply_technology = uint.Parse(msg["power_supply_technology"]);
_present = bool.Parse(msg["present"]);
_serial_number = msg["serial_number"];
}
public PathMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
for (int i = 0; i < msg["poses"].Count; i++)
{
_poses[i] = (new PoseStampedMsg(msg["poses"][i]));
}
}
/**
* C-tor with a member list
*/
public JointStateMsg(HeaderMsg h, string [] n, float [] p, float [] v, float [] e)
{
_header = h;
_name = n;
_position = p;
_velocity = v;
_effort = e;
}
public static void GUI(this HeaderMsg message)
{
#if !ROS2
GUILayout.Label($"<{message.seq} {message.frame_id} {message.stamp.ToTimestampString()}>");
#else
GUILayout.Label($"<{message.frame_id} {message.stamp.ToTimestampString()}>");
#endif
}
public OctomapMsg()
{
this.header = new HeaderMsg();
this.binary = false;
this.id = "";
this.resolution = 0.0;
this.data = new sbyte[0];
}
public BodyVelocityRequestMsg(JSONNode msg)
{
// parse message fields into class data
_header = new HeaderMsg(msg["header"]);
_goal = new GoalDescriptorMsg(msg["goal"]);
_twist = new TwistMsg(msg["twist"]);
_disable_axis = new Bool6AxisMsg(msg["disable_axis"]);
}
public VelocityMsg(HeaderMsg header, double axis_x, double axis_y, double axis_z, double yaw)
{
_header = header;
_axis_x = new Float64Msg(axis_x);
_axis_y = new Float64Msg(axis_y);
_axis_z = new Float64Msg(axis_z);
_yaw = new Float64Msg(yaw);
}
private OctomapMsg(MessageDeserializer deserializer)
{
this.header = HeaderMsg.Deserialize(deserializer);
deserializer.Read(out this.binary);
deserializer.Read(out this.id);
deserializer.Read(out this.resolution);
deserializer.Read(out this.data, sizeof(sbyte), deserializer.ReadLength());
}
public OctomapMsg(HeaderMsg header, bool binary, string id, double resolution, sbyte[] data)
{
this.header = header;
this.binary = binary;
this.id = id;
this.resolution = resolution;
this.data = data;
}
public DetectionArrayMsg(JSONNode msg) {
header = new HeaderMsg(msg["header"]);
origin = new PoseMsg(msg["origin"]);
detections = new DetectionMsg[msg["detections"].Count];
for (int i = 0; i < detections.Length; i++) {
detections[i] = new DetectionMsg(msg["detections"][i]);
}
}
public ImageMsg(HeaderMsg header, uint height, uint width, string encoding, bool is_bigendian, uint row_step, byte[] data)
{
_header = header;
_height = height;
_width = width;
_encoding = encoding;
_is_bigendian = is_bigendian;
_row_step = row_step;
}
public PeopleGoalArrayMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_users = new PersonGoalMsg[msg["users"].Count];
for (int i = 0; i < _users.Length; i++)
{
_users[i] = new PersonGoalMsg(msg["users"][i]);
}
}
public override void Deserialize(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
// Treat poses
for (int i = 0; i < msg["poses"].Count; i++)
{
_poses.Add(new PoseStampedMsg(msg["poses"][i]));
}
}
public SemanticObjectArrayMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_semanticObjects = new SemanticObjectMsg[msg["semanticObjects"].Count];
for (int i = 0; i < _semanticObjects.Length; i++)
{
_semanticObjects[i] = new SemanticObjectMsg(msg["semanticObjects"][i]);
}
}
public override void Deserialize(JSONNode msg)
{
_header = new HeaderMsg(msg ["header"]);
_range = float.Parse(msg ["range"]);
_min_range = float.Parse(msg ["min_range"]);
_max_range = float.Parse(msg ["max_range"]);
_radiation_type = uint.Parse(msg ["radiation_type"]);
_field_of_view = float.Parse(msg ["field_of_view"]);
}
public RangeMsg(HeaderMsg header, float range, float min_range, float max_range, float field_of_view, uint radiation_type)
{
_header = header;
_range = range;
_min_range = min_range;
_max_range = max_range;
_field_of_view = field_of_view;
_radiation_type = radiation_type;
}
public RangeMsg(JSONNode msg)
{
_header = new HeaderMsg(msg ["header"]);
_range = float.Parse(msg ["range"]);
_min_range = float.Parse(msg ["min_range"]);
_max_range = float.Parse(msg ["max_range"]);
_radiation_type = uint.Parse(msg ["radiation_type"]);
_field_of_view = float.Parse(msg ["field_of_view"]);
}
protected void PreloadStrings(Stream input)
{
try
{
if (Strings.Count > 0) Strings.Clear();
_bundleFileHeader = input.ReadStruct<FileHeaderStruct>();
if (_bundleFileHeader.Magic != GETTEXT_MAGIC)
throw new IOException("Invalid MAGIC");
var originalHeaders = ReadStringHeaders(input, _bundleFileHeader.OriginalTableOffset,
_bundleFileHeader.NumberOfStrings);
var translationHeaders = ReadStringHeaders(input, _bundleFileHeader.TranslationTableOffset,
_bundleFileHeader.NumberOfStrings);
var originals = new string[_bundleFileHeader.NumberOfStrings][];
var translations = new string[_bundleFileHeader.NumberOfStrings][];
for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
originals[x] = ReadTextEntry(input, originalHeaders[x].Offset, originalHeaders[x].Length);
for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
translations[x] = ReadTextEntry(input, translationHeaders[x].Offset, translationHeaders[x].Length);
for (var x = 0; x < _bundleFileHeader.NumberOfStrings; x++)
{
switch (originals[x].Length)
{
case 1:
Strings.TryAdd(originals[x][0], translations[x]);
break;
case 2:
Strings.TryAdd(originals[x][0], new[] {translations[x][0]});
Strings.TryAdd(originals[x][1], translations[x]);
break;
}
}
}
finally
{
_headerMsg = new HeaderMsg(GetString(string.Empty));
}
}
