public CollisionPrimitiveMsg(JSONNode msg)
{
_name = msg["name"];
_setup = msg["setup"];
_bbox = new SolidPrimitiveMsg(msg["bbox"]);
_pose = new PoseStampedMsg(msg["pose"]);
}
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 CollisionPrimitiveMsg(string name, string setup, SolidPrimitiveMsg bbox, PoseStampedMsg pose)
{
_name = name;
_setup = setup;
_bbox = bbox;
_pose = pose;
}
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 bool Equals(PoseStampedMsg other)
{
if (other == null)
{
return(false);
}
return(this._header.GetSeq().Equals(other._header.GetSeq()));
}
private void PoseMsgSubscriber_OnCallBack(ROSBridgeMsg msg)
{
pose = (PoseStampedMsg)msg;
drone.transform.rotation = new Quaternion(pose._pose._orientation.GetX(), pose._pose._orientation.GetZ(), pose._pose._orientation.GetY(), pose._pose._orientation.GetW());
drone.transform.position = new Vector3(pose._pose._position.GetX(), pose._pose._position.GetZ(), pose._pose._position.GetY());
sec = pose._header.GetTimeMsg().GetSecs();
nsec = pose._header.GetTimeMsg().GetNsecs();
}
public SceneLabelMsg(PoseStampedMsg pose, float width, float height, string text, string image_filename)
{
_pose = pose;
_width = width;
_height = height;
_text = text;
_image_filename = image_filename;
}
public SceneLabelMsg(JSONNode msg)
{
_pose = new PoseStampedMsg(msg["pose"]);
_width = msg["width"].AsFloat;
_height = msg["height"].AsFloat;
_text = msg["test"];
_image_filename = msg["image_filename"];
}
public PathMsg(JSONNode msg)
{
_header = new HeaderMsg(msg["header"]);
_poses = new PoseStampedMsg[msg["poses"].Count];
for (int i = 0; i < _poses.Length; i++)
{
_poses[i] = new PoseStampedMsg(msg["poses"][i]);
}
}
public void Update()
{
connection.Render();
//TODO abstract the "fcu" string
HeaderMsg header = new HeaderMsg(seq++, RosTime.Now, "fcu");
PoseStampedMsg poseStampedMsg = new PoseStampedMsg(header, PoseDesired);
connection.Publish(MavrosSetpointPositionLocalPublisher.GetMessageTopic(), poseStampedMsg);
}
IEnumerator PubOwnPosition()
{
string pose_topic = string.Concat("/car", Process.GetCurrentProcess().Id, "/car", netId.ToString(), "/position");
ros.AddPublisherNew(typeof(PoseStampedMsg), pose_topic);
// ros.Connect();
while (true)
{
if (isLocalPlayer)
{
// if(start_flag){
UnityEngine.Debug.Log("local player");
UnityEngine.Debug.Log(inputField.text);
Vector3 bar = transform.position;
PoseStampedMsg msg = new PoseStampedMsg(new HeaderMsg(0, new TimeMsg(0, 0), "cam"), new PoseMsg(new PointMsg(bar.x, bar.z, 0), new QuaternionMsg(0, 0, 0, 0)));
UnityEngine.Debug.Log(pose_topic);
ros.Publish(pose_topic, msg);
yield return(null);
// }
}
}
}
public static string GetMessageType()
{
return(PoseStampedMsg.GetMessageType());
}
public MoveBaseGoalMsg(PoseStampedMsg target_pose)
{
_target_pose = target_pose;
}
public MoveBaseGoalMsg(JSONNode msg)
{
_target_pose = new PoseStampedMsg(msg["target_pose"]);
}
// 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));
}
}
