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(PoseMsgPublisher.GetMessageTopic(), poseStampedMsg);
}
// Update is called once per frame
void Update()
{
//unix = DateTimeOffset.Now.ToUnixTimeSeconds();
TimeSpan t = DateTime.UtcNow - new DateTime(1970, 1, 1);
secondsSinceEpoch = (int)t.TotalSeconds;
nSeconds = t.Milliseconds * 1000000;
connection.Render();
float xOffset = 0;
if (Input.GetKeyDown(KeyCode.Return))
{
connection.CallService("mavros/cmd/arming", "[true]");
connection.CallService("/mavros/set_mode", "[0, \"OFFBOARD\"]");
}
if (Input.GetKeyDown(KeyCode.Escape))
{
connection.CallService("mavros/cmd/arming", "[false]");
}
if (Input.GetKey(KeyCode.LeftArrow))
{
xOffset--;
}
if (Input.GetKey(KeyCode.RightArrow))
{
xOffset++;
}
connection.Publish(
PoseMsgPublisher.GetMessageTopic(),
new PoseStampedMsg(
new HeaderMsg(
seq,
new TimeMsg(
pose._header.GetTimeMsg().GetSecs(), //secondsSinceEpoch,
pose._header.GetTimeMsg().GetNsecs() + 10), //nSeconds),
pose._header.GetFrameId()), //"fcu"),
new PoseMsg(
new PointMsg(pose._pose._position.GetX() + xOffset, pose._pose._position.GetY(), 2), //pose._pose._position.GetZ()),
pose._pose._orientation
)
));
seq++;
}
