File:Exampledriver.cc.zip

/*

*  Player - One Hell of a Robot Server
*  Copyright (C) 2003  
*     Brian Gerkey
*                      
* 
*  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
*
*/

/*

* A simple example of how to write a driver that will be built as a
* shared object.
*/

// ONLY if you need something that was #define'd as a result of configure // (e.g., HAVE_CFMAKERAW), then #include <config.h>, like so: /*

1. if HAVE_CONFIG_H

 #include <config.h>

1. endif

• /

1. include <unistd.h>
2. include <string.h>

1. include <libplayercore/playercore.h>

//////////////////////////////////////////////////////////////////////////////// // The class for the driver class ExampleDriver : public Driver {

 public:
   
   // Constructor; need that
   ExampleDriver(ConfigFile* cf, int section);

   // Must implement the following methods.
   virtual int Setup();
   virtual int Shutdown();

   // This method will be invoked on each incoming message
   virtual int ProcessMessage(MessageQueue* resp_queue, 
                              player_msghdr * hdr,
                              void * data);

 private:

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

   int foop;

};

// A factory creation function, declared outside of the class so that it // can be invoked without any object context (alternatively, you can // declare it static in the class). In this function, we create and return // (as a generic Driver*) a pointer to a new instance of this driver. Driver* ExampleDriver_Init(ConfigFile* cf, int section) {

 // Create and return a new instance of this driver
 return((Driver*)(new ExampleDriver(cf, section)));

}

// A driver registration function, again declared outside of the class so // that it can be invoked without object context. In this function, we add // the driver into the given driver table, indicating which interface the // driver can support and how to create a driver instance. void ExampleDriver_Register(DriverTable* table) {

 table->AddDriver("exampledriver", ExampleDriver_Init);

}

//////////////////////////////////////////////////////////////////////////////// // Constructor. Retrieve options from the configuration file and do any // pre-Setup() setup. ExampleDriver::ExampleDriver(ConfigFile* cf, int section)

   : Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, 
            PLAYER_POSITION2D_CODE)

{

 // Read an option from the configuration file
 this->foop = cf->ReadInt(section, "foo", 0);

 return;

}

//////////////////////////////////////////////////////////////////////////////// // Set up the device. Return 0 if things go well, and -1 otherwise. int ExampleDriver::Setup() {

 puts("Example driver initialising");

 // Here you do whatever is necessary to setup the device, like open and
 // configure a serial port.

 printf("Was foo option given in config file? %d\n", this->foop);
   
 puts("Example driver ready");

 // Start the device thread; spawns a new thread and executes
 // ExampleDriver::Main(), which contains the main loop for the driver.
 StartThread();

 return(0);

}

//////////////////////////////////////////////////////////////////////////////// // Shutdown the device int ExampleDriver::Shutdown() {

 puts("Shutting example driver down");

 // Stop and join the driver thread
 StopThread();

 // Here you would shut the device down by, for example, closing a
 // serial port.

 puts("Example driver has been shutdown");

 return(0);

}

int ExampleDriver::ProcessMessage(MessageQueue* resp_queue,

                                 player_msghdr * hdr,
                                 void * data)

{

 // Process messages here.  Send a response if necessary, using Publish().
 // If you handle the message successfully, return 0.  Otherwise,
 // return -1, and a NACK will be sent for you, if a response is required.
 return(0);

}

//////////////////////////////////////////////////////////////////////////////// // Main function for device thread void ExampleDriver::Main() {

 // The main loop; interact with the device here
 for(;;)
 {
   // test if we are supposed to cancel
   pthread_testcancel();

   // Process incoming messages.  ExampleDriver::ProcessMessage() is
   // called on each message.
   ProcessMessages();

   // Interact with the device, and push out the resulting data, using
   // Driver::Publish()

   // Sleep (you might, for example, block on a read() instead)
   usleep(100000);
 }

}

//////////////////////////////////////////////////////////////////////////////// // Extra stuff for building a shared object.

/* need the extern to avoid C++ name-mangling */ extern "C" {

 int player_driver_init(DriverTable* table)
 {
   puts("Example driver initializing");
   ExampleDriver_Register(table);
   puts("Example driver done");
   return(0);
 }

}