/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include "../headers/MessageStructures.hh"
#include "SPOutLocalizationEncoder.h"


int
SPOutLocalizationEncoder::encodeLocalization(uchar *buf, RobotPositionInfo *posn_info) {
  uchar *orig_buf=buf;

  /* Encode the new localization state */
  encodeAs<float>(&buf,(float)(posn_info->position.mean.x));
  encodeAs<float>(&buf,(float)(posn_info->position.mean.y));
  encodeAs<float>(&buf,(float)(posn_info->position.thetaMaj));
  encodeAs<float>(&buf,(float)(posn_info->position.sMaj));
  encodeAs<float>(&buf,(float)(posn_info->position.sMin));
  encodeAs<float>(&buf,(float)(posn_info->heading.mean.angle()));
  encodeAs<float>(&buf,(float)(posn_info->heading.sMaj));
  return buf-orig_buf;
}