How to write your own sensor simulator ?

:!: DEPRECATED Information

This document explains how to simulate a new sensor. In our example we'll simulate a compass wich give the absolute angle of the robot in the virtual world.

Creation of the shared variable

Create the output data structure

First you need to create the structure of the shared variable, this is the output data of the sensor which will be in the shared memory and accessible by other process using the Hugr library. The definition a such structure is to be saved in Structs/yourdataStruct.hpp file Example : Structs/CompassStruct.hpp

class Compass
{
        public:
                double theta;
};
Write the serialization method

The serialization method is needed to write the data in the shared memory. The serialization uses the boost serialization library which proposes serialization methods for most used types, for example : int, char, bool[15], std::string, std::list.

namespace boost
{
        namespace serialization
        {
                template<class Archive> void serialize(Archive &ar, Compass &c, const unsigned int version)
                {
                        ar & c.theta;
                }
        }
}

Simulate the sensor in mgEngine

  • First you can write the header file.
The sensor object needs to inherit from the class VirtualSensor
  • Then you need to implement the methods :
    • void init( const std::string& fileName )

      which initializes the sensor given where fileName is the config file

    • void reinit()

      method to reinitialize the sensor

    • void doAcquisition( double deltaT )

      method which is called at each simulation iteration. The object needs to contain an instance of hugr::Store to write the output data in the shared memory.

Example :

#ifndef __COMPASS_SIMULATOR_HPP
#define __COMPASS_SIMULATOR_HPP
 
#include "VirtualSensor.hpp"
 
#define DLC_DECLARED_COMPONENT "CompassSimulator"
#include <dlc/dlcDynamic.h>
#include <hugr.hpp>
 
#include "CompassStruct.hpp"
 
class DLC_DYNAMIC CompassSimulator :
        public VirtualSensor
{
private:
        hugr::Store store;
        hugr::VariableId compassId;
        Compass compass;
 
public :
        CompassSimulator( const std::string & Name );
        void init( const std::string& fileName );
        void reinit();
        void doAcquisition( double deltaT );
};
#endif

Now you write the implementation, the interesting points are how we register the shared variable and write it to the store, and how we get the 3d position of the sensor.

#include "CompassSimulator.hpp"
 
using namespace std;
using namespace hugr;
 
#define DLC_COMPILED_COMPONENT "CompassSimulator"
#include <dlc/dlc.h>
 
DLC_FACTORY_1( mgScriptable, CompassSimulator, const string& );
 
CompassSimulator::CompassSimulator( const string &name ) : VirtualSensor(name)
{
        compass.theta = 0;
        store.connect(); //connect to the store
        try
        {
                //try to find the variable
                compassId = store.lookupVariable( name+"_compass" );
        }
        catch(runtime_error &e)
        {
                //if the variable doesn't exit the exeption is cought and
                //we create a new variable
                compassId = store.registerVariable(name+"_compass", compass);
        }
}
 
void CompassSimulator::init( const std::string& fileName ) {}
 
void CompassSimulator::reinit() {}
 
 
void CompassSimulator::doAcquisition( double deltaT )
{
        //retrieve the global rotation over the Z axis
        //CoordFrame contain the position of the sensor
        compass.theta = CoordFrame()->GlobalQuaternion().GetFixedAngles().z;
 
        //and now we write it to the shared memory
        store.writeVariable(compassId, compass);
}

Now you can compile your code and create a dynamic library.

gcc -I. -IpathwheretofindDirStructs -I/usr/include/sigc++-2.0 -I/usr/lib/sigc++-2.0/include -fPIC -c CompassSimulator.o -o CompassSimulator.o
gcc -shared -Wl,-soname,libCompassSimulator.so.1 -o libCompassSimulator.so.1.0.1 CompassSimulator.o -LpathtofindlibrarieshugrandVirtualSensor -lhugr -lVirtualSensor -lboost_serialization-mt

Using the sensor in mgEngine

To use the sensor created in the previous part, you need to import it in mgEngine, to do that, you can both use the mgEngine's shell or you can edit a script.

  • First you need to declare the sensor and init it :
new CompassSimulator compass
compass.Init
  • Then you attach it to the robot you declared previously (in our example we attach the sensor to a cycab):
compass.MakeChildOf cycab_mesh
  • You can set the sensor position in the robot reference :
compass.SetPosition 1.68 0 0.7
compass.SetRotation 0 0 0
  • Finally, you start the simulation :
compass.Start
If you want to go further and visualise the sensor you can attach a mesh to the sensor, see the section How to design a new 3D model for the simulator ?
deprecated/howto/devel-sensor.txt · Last modified: 2012/01/24 15:44 by arias
Recent changes RSS feed Creative Commons License Donate Minima Template by Wikidesign Driven by DokuWiki