odometrystate.cpp

一个简单使用的控制器

#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.
 *********************************************************************/