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 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"]);
}
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 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]);
}
// 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));
}
}
