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));
}
}
IEnumerator SendImages(ROS ros)
{
HeaderMsg _head;
Log("Sending images to ros.", LogLevel.Developer);
while (Application.isPlaying)
{
if (ros.IsConnected())
{
_head = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), transform.name);
ros.Publish(CameraRGB_pub.GetMessageTopic(), new CompressedImageMsg(_head, "jpeg", CaptureImage(ImageType.RGB).EncodeToJPG()));
ros.Publish(CameraDepth_pub.GetMessageTopic(), new CompressedImageMsg(_head, "png", CaptureImage(ImageType.Depth).EncodeToPNG()));
}
yield return(new WaitForSeconds(1 / ROSFrecuency));
}
yield return(null);
}
private IEnumerator SendLaser(ROS ros)
{
Log("Sending laser to ros.", LogLevel.Developer);
while (ros.IsConnected())
{
yield return(new WaitForEndOfFrame());
HeaderMsg _head = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), transform.name);
LaserScanMsg scan = new LaserScanMsg(_head, angleMin * Mathf.Deg2Rad, angleMax * Mathf.Deg2Rad, angleIncrement * Mathf.Deg2Rad, 0, 0, rangeMin, rangeMax, Scan(), new double[0]);
ros.Publish(LaserScan_pub.GetMessageTopic(), scan);
yield return(new WaitForSeconds(ROSFrecuency));
}
}
IEnumerator SendImages()
{
while (true)
{
Texture2D rgb = RTImageColor(_cameras[0]);
Texture2D depth = RTImageDepth(_cameras[1]);
//Color c;
//for (int i = 0; i < rgb.width; i++) {
// for (int j = 0; j < rgb.height; j++) {
// c = rgb.GetPixel(i, j);
// c.a = depth.GetPixel(i, j).a;
// rgb.SetPixel(i, j, c);
// }
//}
Color[] pxs = rgb.GetPixels();
Color[] pxsDepth = depth.GetPixels();
for (int i = 0; i < pxs.Length; i++)
{
pxs[i].a = pxsDepth[i].a;
}
rgb.SetPixels(pxs);
HeaderMsg _head = new HeaderMsg(0, new TimeMsg(_ros.GetepochStart().Second, 0), "map");
//RGB
CompressedImageMsg compressedImg = new CompressedImageMsg(_head, "png", rgb.EncodeToPNG());
_ros.Publish(RobotCamera_pub.GetMessageTopic(), compressedImg);
//Depth
//compressedImg = new CompressedImageMsg(_head, "png", RTImageDepth(_cameras[1]).EncodeToPNG());
//_ros.Publish(CameraDepth_pub.GetMessageTopic(), compressedImg);
yield return(new WaitForSeconds(_frecuency));
}
}
// Send Odometry to ROS
private IEnumerator SendOdometryToROS(ROS ros)
{
while (Application.isPlaying)
{
if (ros.IsConnected())
{
double[] covariance_pose = new double[36];
for (int i = 0; i < covariance_pose.Length; i++)
{
covariance_pose[i] = 0.0f;
}
HeaderMsg globalHead = new HeaderMsg(0, new TimeMsg(DateTime.Now.Second, 0), "map");
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(new PoseMsg(new PointMsg(transform.position, true), new QuaternionMsg(transform.rotation, true)),
covariance_pose);
TwistWithCovarianceMsg twist = new TwistWithCovarianceMsg(new TwistMsg(new Vector3Msg(agent.velocity, true), new Vector3Msg(0.0f, 0.0f, 0.0f)), covariance_pose);
OdometryMsg odometrymsg = new OdometryMsg(globalHead, "odom", pose, twist);
ros.Publish(Odometry_pub.GetMessageTopic(), odometrymsg);
}
yield return(new WaitForSeconds(rOSFrecuencyOdometry));
}
}
// This function was design by: Jose Luiz Matez
private IEnumerator SendInterpolatedPathToROS(ROS ros)
{
while (Application.isPlaying)
{
if (ros.IsConnected())
{
Vector3[] points = agent.path.corners;
Vector3[] path = new Vector3[6];
float[] distances = new float[points.Length];
float[] angles = new float[points.Length];
PoseStampedMsg[] poses = new PoseStampedMsg[path.Length];
for (int i = 0; i < (points.Length - 1); i++)
{
if (i == 0)
{
distances[i] = Vector3.Distance(transform.position, points[i + 1]);
}
else
{
distances[i] = Vector3.Distance(points[i], points[i + 1]);
}
angles[i] = Mathf.Atan2(points[i + 1].z - points[i].z, points[i + 1].x - points[i].x);
}
path[0] = transform.position;
HeaderMsg head = new HeaderMsg(0, new TimeMsg(DateTime.Now.AddSeconds(0).Second, 0), "map");
Quaternion rotation = transform.rotation;
poses[0] = new PoseStampedMsg(head, new PoseMsg(path[0], rotation, true));
head.SetSeq(0);
for (int j = 1; j < 6; j++)
{
float accumulatedDistance = 0.0f;
int idx = 0;
while (accumulatedDistance < agent.velocity.magnitude && idx < distances.Length)
{
if (idx < distances.Length)
{
if (distances[idx] >= (agent.velocity.magnitude - accumulatedDistance))
{
distances[idx] = distances[idx] - (agent.velocity.magnitude - accumulatedDistance);
accumulatedDistance = accumulatedDistance + (agent.velocity.magnitude - accumulatedDistance);
}
else
{
accumulatedDistance = accumulatedDistance + distances[idx];
distances[idx] = 0.0f;
if (idx == (distances.Length - 1))
{
HeaderMsg head_path = new HeaderMsg(0, new TimeMsg(DateTime.Now.AddSeconds(j).Second, 0), "map");
Quaternion add_rotation = Quaternion.Euler(0.0f, angles[idx], 0.0f);
rotation = rotation * add_rotation;
head.SetSeq(j);
path[j] = new Vector3(path[j - 1].x + accumulatedDistance * Mathf.Cos(angles[idx]), 0.0f, path[j - 1].z + accumulatedDistance * Mathf.Sin(angles[idx]));
poses[j] = new PoseStampedMsg(head_path, new PoseMsg(path[j], rotation, true));
}
}
if (accumulatedDistance == agent.velocity.magnitude)
{
HeaderMsg head_path = new HeaderMsg(0, new TimeMsg(DateTime.Now.AddSeconds(j).Second, 0), "map");
Quaternion add_rotation = Quaternion.Euler(0.0f, angles[idx], 0.0f);
rotation = rotation * add_rotation;
head.SetSeq(j);
path[j] = new Vector3(path[j - 1].x + accumulatedDistance * Mathf.Cos(angles[idx]), 0.0f, path[j - 1].z + accumulatedDistance * Mathf.Sin(angles[idx]));
poses[j] = new PoseStampedMsg(head_path, new PoseMsg(path[j], rotation, true));
}
idx = idx + 1;
}
else
{
accumulatedDistance = agent.velocity.magnitude;
path[j] = path[j - 1];
HeaderMsg head_path = new HeaderMsg(0, new TimeMsg(DateTime.Now.AddSeconds(j).Second, 0), "map");
head.SetSeq(j);
path[j] = new Vector3(path[j - 1].x + accumulatedDistance * Mathf.Cos(angles[idx]), 0.0f, path[j - 1].z + accumulatedDistance * Mathf.Sin(angles[idx]));
poses[j] = new PoseStampedMsg(head_path, new PoseMsg(path[j], rotation, true));
}
}
}
if (poses[1] != null)
{
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));
}
}