#include "OdometryState.h"#include <iostream>#include "util/robotId.h"#include "com/ipc_com.h"#define DEBUG (0)/********************************************** * msgHandlerKalmanPoseMessage **********************************************/static void msgHandlerKalmanPose(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_kalman_pose_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(rescue_kalman_pose_message));   IPC_freeByteArray(callData);/*   if (msg.robot.id != getRobotId()) {      if (DEBUG)         rlogDebug( "Received message for other robot (%d) while me id is %d", msg.robot.id,getRobotId());      return;   }*/   if (DEBUG) {      rlogDebug("\nGot KalmanPose message from client : time: %ld,%ld\n" , msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);      rlogDebug( "Pose (x,y,th): (%f,%f,%f)",msg.posX, msg.posZ, msg.posTh);   }   OdometryState* aState = ((OdometryState*)clientData);   aState->update(msg);}/********************************************** * msgHandlerOdometryMessage **********************************************/static void msgHandlerOdometry(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_odometry_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(rescue_odometry_message));   IPC_freeByteArray(callData);   if (DEBUG) {      rlogDebug( "\nGot Odometry message from client : time: %ld,%ld\n" , msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);      rlogDebug( "Pose (x,y,th): (%d,%d,%d)\n",msg.posX, msg.posZ, msg.posTh);   }   OdometryState* aState = ((OdometryState*)clientData);   aState->update(msg);}/********************************************** * msgHandlerInertiaMessage **********************************************/static void msgHandlerInertia(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_inertiacube_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(msg));   IPC_freeByteArray(callData);   if (DEBUG) {      rlogDebug("\nGot Inertia message from client : time: %ld,%ld\n" , msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);      rlogDebug( "Pose (x,y,th): (%f,%f,%f)\n",msg.pitch, msg.roll, msg.yaw);   }   OdometryState* aState = ((OdometryState*)clientData);   aState->update(msg);}OdometryState::OdometryState(bool useKalmanPoses) {   // Prepare client for reading   if (!useKalmanPoses)      ComSubscribeToRobot(RESCUE_ODOMETRY_NAME, msgHandlerOdometry, this);   else      ComSubscribeToRobot(RESCUE_KALMAN_POSE_NAME, msgHandlerKalmanPose, this);      ComSubscribeToRobot(RESCUE_INERTIA_NAME, msgHandlerInertia, this);   _yawSet=false;}OdometryState::~OdometryState() {}OdometryState* OdometryState::singleton = NULL;void OdometryState::update(rescue_odometry_message &msg) {   m_vars.clear();   double angle;   if(_yawSet) //FIXME	angle=_inuYaw;   else	angle=msg.posTh;   m_vars.push_back((int) msg.posX);   m_vars.push_back((int) msg.posZ);   m_vars.push_back((int) angle);   m_vars.push_back((int) msg.transVel);   m_vars.push_back((int) msg.rotVel);   timestamp = msg.robot.ts;}void OdometryState::update(rescue_kalman_pose_message &msg) {   m_vars.clear();   double angle;   if(_yawSet) //FIXME	angle=_inuYaw;   else	angle=msg.posTh;   m_vars.push_back((int) msg.posX);   m_vars.push_back((int) msg.posZ);   m_vars.push_back((int) angle);   m_vars.push_back((int) msg.transVel);   m_vars.push_back((int) msg.rotVel);   timestamp = msg.robot.ts;}void OdometryState::update(rescue_inertiacube_message &msg) {   _inuYaw= msg.yaw; //FIXME   _yawSet=true;}int OdometryState::getPosX() {   if(m_vars.size() <= 0)      return 0;   return m_vars[0];}int OdometryState::getPosZ() {     if(m_vars.size() <= 1)        return 0;     return m_vars[1];}int OdometryState::getPosTh() {   if(m_vars.size() <= 2)      return 0;   return m_vars[2];}int OdometryState::getTVel() {   if(m_vars.size() <= 3)      return 0;   return m_vars[3];}int OdometryState::getRVel() {   if(m_vars.size() <= 4)      return 0;   return m_vars[4];}/********************************************************************* * (C) Copyright 2006 Albert Ludwigs University Freiburg *     Institute of Computer Science * * All rights reserved. Use of this software is permitted for * non-commercial research purposes, and it may be copied only * for that use. All copies must include this copyright message. * This software is made available AS IS, and neither the authors * nor the Albert Ludwigs University Freiburg make any warranty * about the software or its performance. *********************************************************************/