inertiacube2.cc

机器人仿真平台,和stage配合运行

/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000  Brian Gerkey   &  Kasper Stoy
 *                      [e-mail]    [e-mail]
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */
///////////////////////////////////////////////////////////////////////////
//
// Desc: Driver ISense InertiaCube2 orientation sensor.
// Author: Andrew Howard
// Date: 20 Aug 2002
// CVS: $Id: inertiacube2.cc,v 1.1 2002/11/29 17:08:13 gerkey Exp $
//
// Theory of operation:
//  Uses an inertial orientation sensor to correct the odometry coming
//  from a robot.  The assumption is that the position device we
//  subscribe to has good position information but poor orientation
//  information.
//
// Requires: position
// Provides: position
//
///////////////////////////////////////////////////////////////////////////

#include <errno.h>
#include <string.h>
#include <math.h>
#include <stdlib.h>       // for atoi(3)
#include <netinet/in.h>   // for htons(3)
#include <unistd.h>

#include <isense/isense.h>

#include "player.h"
#include "device.h"
#include "devicetable.h"
#include "drivertable.h"


// Driver for detecting laser retro-reflectors.
class InertiaCube2 : public CDevice
{
  // Constructor
  public: InertiaCube2(char* interface, ConfigFile* cf, int section);

  // Setup/shutdown routines.
  public: virtual int Setup();
  public: virtual int Shutdown();

  // Set up the underlying position device.
  private: int SetupPosition();
  private: int ShutdownPosition();

  // Initialize the IMU.
  private: int SetupImu();
  private: int ShutdownImu();

  // Get the tracker type.
  private: const char *ImuType(int type);

  // Get the tracker model.
  private: const char *ImuModel(int model);

  // Main function for device thread.
  private: virtual void Main();

  // Process requests.  Returns 1 if the configuration has changed.
  private: int HandleRequests();

  // Handle geometry requests.
  private: void HandleGetGeom(void *client, void *req, int reqlen);

  // Update the InertiaCube.
  private: void UpdateImu();

  // Update the position device (returns non-zero if changed).
  private: int UpdatePosition();

  // Generate a new pose estimate.
  private: void UpdatePose();

  // Update the device data (the data going back to the client).
  private: void UpdateData();

  // Geometry of underlying position device.
  private: player_position_geom_t geom;

  // Compass setting (0 = off, 1 = partial, 2 = full).
  private: int compass;
  
  // Serial port.
  private: const char *port;

  // Position device info (the one we are subscribed to).
  private: int position_index;
  private: CDevice *position;
  private: double position_time;
  private: double position_old_pose[3];
  private: double position_new_pose[3];

  // Handle to the imu tracker.
  private: ISD_TRACKER_HANDLE imu;
  private: double imu_old_orient;
  private: double imu_new_orient;

  // Combined pose estimate.
  private: double pose[3];
};


// Initialization function
CDevice* InertiaCube2_Init(char* interface, ConfigFile* cf, int section)
{
  if (strcmp(interface, PLAYER_POSITION_STRING) != 0)
  {
    PLAYER_ERROR1("driver \"inertiacube2\" does not support interface \"%s\"\n",
                  interface);
    return (NULL);
  }
  return ((CDevice*) (new InertiaCube2(interface, cf, section)));
}


// a driver registration function
void InertiaCube2_Register(DriverTable* table)
{
  table->AddDriver("inertiacube2", PLAYER_READ_MODE, InertiaCube2_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
InertiaCube2::InertiaCube2(char* interface, ConfigFile* cf, int section)
    : CDevice(sizeof(player_fiducial_data_t), 0, 10, 10)
{
  this->port = cf->ReadString(section, "port", "/dev/ttyS3");
  
  this->position_index = cf->ReadInt(section, "position_index", 0);
  this->position = NULL;
  this->position_time = 0;

  this->compass = cf->ReadInt(section, "compass", 2);
                              
  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int InertiaCube2::Setup()
{
  // Initialise the underlying position device.
  if (this->SetupPosition() != 0)
    return -1;
  
  // Initialise the cube.
  if (this->SetupImu() != 0)
    return -1;

  // Start the driver thread.
  this->StartThread();
  
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int InertiaCube2::Shutdown()
{
  // Stop the driver thread.
  this->StopThread();

  // Stop the imu.
  this->ShutdownImu();

  // Stop the position device.
  this->ShutdownPosition();

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the underlying position device.
int InertiaCube2::SetupPosition()
{
  player_device_id_t id;

  id.code = PLAYER_POSITION_CODE;
  id.index = this->position_index;
  id.port = this->device_id.port;

  // Subscribe to the position device.
  this->position = deviceTable->GetDevice(id);
  if (!this->position)
  {
    PLAYER_ERROR("unable to locate suitable position device");
    return -1;
  }
  if (this->position->Subscribe(this) != 0)
  {
    PLAYER_ERROR("unable to subscribe to position device");
    return -1;
  }

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
int InertiaCube2::ShutdownPosition()
{
  // Unsubscribe from devices.
  this->position->Unsubscribe(this);

  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Initialize the imu.
int InertiaCube2::SetupImu()
{
  int i;
  int port;
  int verbose;
  ISD_TRACKER_INFO_TYPE info;
  ISD_STATION_INFO_TYPE sinfo;
  ISD_TRACKER_DATA_TYPE data;
  
  verbose = 0;
  
  // Open the tracker.  Takes a port number, so we strip it from the
  // port string.
  port = atoi(this->port + strlen(this->port) - 1);
  this->imu = ISD_OpenTracker((Hwnd) NULL, port + 1, FALSE, verbose);
  if (this->imu < 1)
  {
    PLAYER_ERROR("failed to detect InterSense tracking device");
    return -1;
  }

  // Get tracker configuration info
  if (!ISD_GetTrackerConfig(this->imu, &info, verbose))
  {
    PLAYER_ERROR("failed to get configuration info");
    return -1;
  }

  printf("InterSense Tracker type [%s] model [%s]\n",
         this->ImuType(info.TrackerType), this->ImuModel(info.TrackerModel));

  // Get some more configuration info.
  if (!ISD_GetStationConfig(this->imu, &sinfo, 1, verbose))
  {
    PLAYER_ERROR("failed to get station info");
    return -1;
  }

  // Set compass value (0 = off, 1 = partial, 2 = full).
  sinfo.Compass = this->compass;

  printf("compass %d enhancement %d sensitivity %d prediction %d format %d\n",
         sinfo.Compass, sinfo.Enhancement, sinfo.Sensitivity,
         sinfo.Prediction, sinfo.AngleFormat);

  // Change the configuration.
  if (!ISD_SetStationConfig(this->imu, &sinfo, 1, verbose))
  {
    PLAYER_ERROR("failed to get station info");
    return -1;
  }

  // Wait a while for the unit to settle.
  for (i = 0; i < 100;)
  {
    if (!ISD_GetData(this->imu, &data))
    {
      PLAYER_ERROR("failed to get data");
      return -1;
    }
    if (data.Station[0].NewData)
      i++;
    usleep(10000);
  }

  // Reset the heading component.
  if (!ISD_ResetHeading(this->imu, 1))
  {