fakelocalize.cc

机器人仿真软件

/*
 *  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 for faking localization data from simulation ground truth
// Author: Brian Gerkey
// Date: 4 April 2005
// CVS: $Id: fakelocalize.cc,v 1.7 2006/01/20 01:12:55 gerkey Exp $
//
// Requires - Localize device.
//
///////////////////////////////////////////////////////////////////////////

/** @ingroup drivers */
/** @{ */
/** @defgroup driver_fakelocalize fakelocalize
 * @brief Fake localization

The fakelocalize driver polls a simulation device for 2D ground truth pose
data, then reports this data as if it were generated by a localization
system.  This driver is useful for running software (e.g., @ref 
util_playernav, @ref driver_wavefront) that needs a @ref
interface_localize device without incurring the computational
cost of actually running a localization algorithm.

@par Compile-time dependencies

- none

@par Provides

- @ref interface_localize

@par Requires

- @ref interface_simulation : the device from which to get ground truth

@par Configuration requests

- PLAYER_LOCALIZE_SET_POSE_REQ : acknowledged, but ignored
  
@par Configuration file options

- model (string)
  - Default: NULL
  - Name of simulation model for which we're faking localization.
      
@par Example 

@verbatim
driver
(
  name "stage"
  provides ["6665:simulation:0"]
  plugin "libstage"
  worldfile "foo.world"
)
driver
(
  name "fakelocalize"
  provides ["6665:localize:0"]
  requires ["6665:simulation:0"]
  # a model (probably position) declared in "foo.world"
  model "robot0"
)
@endverbatim

@author Brian Gerkey

*/
/** @} */


#include <sys/types.h>
#include <netinet/in.h>
#include <unistd.h>

#include <libplayercore/playercore.h>

#define SLEEPTIME_US 100000

#define min(x,y) (x < y ? x : y)

// Driver for computing the free c-space from a laser scan.
class FakeLocalize : public Driver
{
  // Constructor
  public: FakeLocalize( ConfigFile* cf, int section);

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

  // Simulation stuff.
  private: int UpdateData();
  private: void HandleRequests();
  private: Device *sim;
  private: player_devaddr_t sim_id;
  private: const char* model;
};


// Initialization function
Driver* FakeLocalize_Init( ConfigFile* cf, int section)
{
  return ((Driver*) (new FakeLocalize( cf, section)));
}


// a driver registration function
void FakeLocalize_Register(DriverTable* table)
{
  table->AddDriver("fakelocalize", FakeLocalize_Init);
}


////////////////////////////////////////////////////////////////////////////////
// Constructor
FakeLocalize::FakeLocalize( ConfigFile* cf, int section)
  : Driver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_LOCALIZE_CODE)
{
  // Must have an input sim
  if (cf->ReadDeviceAddr(&this->sim_id, section, "requires",
                       PLAYER_SIMULATION_CODE, -1, NULL) != 0)
  {
    this->SetError(-1);    
    return;
  }
  this->sim = NULL;

  if(!(this->model = cf->ReadString(section, "model", NULL)))
  {
    PLAYER_ERROR("must specify non-NULL model name");
    this->SetError(-1);
    return;
  }
  if(strlen(this->model) >= PLAYER_SIMULATION_IDENTIFIER_MAXLEN)
  {
    PLAYER_ERROR("model name is too long");
    this->SetError(-1);
    return;
  }

  return;
}


////////////////////////////////////////////////////////////////////////////////
// Set up the device (called by server thread).
int FakeLocalize::Setup()
{
  // Subscribe to the sim.
  if(!(this->sim = deviceTable->GetDevice(this->sim_id)))
  {
    PLAYER_ERROR("unable to locate suitable simulation device");
    return(-1);
  }
  if(this->sim->Subscribe(this->InQueue) != 0)
  {
    PLAYER_ERROR("unable to subscribe to simulation device");
    return(-1);
  }


/*  if(this->UpdateData() < 0)
  {
    PLAYER_ERROR("unable to get pose from simulation device");
    this->sim->Unsubscribe(this->InQueue);
    	sim = NULL;
//    UnsubscribeInternal(this->sim_id);
    return(-1);
  }*/
  this->StartThread();
  return 0;
}


////////////////////////////////////////////////////////////////////////////////
// Shutdown the device (called by server thread).
int FakeLocalize::Shutdown()
{
  // Unsubscribe from devices.
  this->sim->Unsubscribe(this->InQueue);
  sim = NULL;
  //UnsubscribeInternal(this->sim_id);
  
  return 0;
}

int
FakeLocalize::UpdateData()
{
  player_localize_data_t loc_data;
  player_simulation_pose2d_req_t cfg;
//  int replen;
  unsigned short reptype;
//  struct timeval ts;
  
  // Request pose
  strncpy(cfg.name, this->model, PLAYER_SIMULATION_IDENTIFIER_MAXLEN);
  cfg.name[PLAYER_SIMULATION_IDENTIFIER_MAXLEN - 1] = '\0';
  cfg.name_count = min(strlen(cfg.name),PLAYER_SIMULATION_IDENTIFIER_MAXLEN-1) + 1;

  Message * Reply = sim->Request(InQueue, PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D,
  		(void *) &cfg, sizeof(cfg), NULL);
  		
  if (Reply && Reply->GetHeader()->type == PLAYER_MSGTYPE_RESP_ACK)
  {
  	// we got a good reply so update our data
  	assert(Reply->GetPayloadSize() == sizeof(cfg));
  	player_simulation_pose2d_req_t * resp = reinterpret_cast<player_simulation_pose2d_req_t *> (Reply->GetPayload());
  	
    // Fill in loc_data, byteswapping as we go.
    loc_data.pending_count = 0;
    loc_data.pending_time = Reply->GetHeader()->timestamp;
    loc_data.hypoths_count = 1;

    loc_data.hypoths[0].mean = resp->pose;

    // zero covariance and max weight
    memset(loc_data.hypoths[0].cov,0,sizeof(int64_t)*9);
    loc_data.hypoths[0].alpha = htonl((uint32_t)1e6);

    Publish(device_addr, NULL, PLAYER_MSGTYPE_DATA, PLAYER_LOCALIZE_DATA_HYPOTHS,&loc_data,sizeof(loc_data));
    delete Reply;
    	Reply = NULL;
  }
  else
  {
  	// we got an error, thats bad
  	delete Reply;
  	return -1;
  }
    

  return(0);
}


void
FakeLocalize::Main()
{
  for(;;)
  {
    if(this->UpdateData() < 0)
    {
      PLAYER_ERROR("failed to get pose from simulation device");
      pthread_exit(NULL);
    }
    //HandleRequests();
    usleep(SLEEPTIME_US);
  }
}