Creating a new sensor

From UWSim
Revision as of 17:01, 11 January 2017 by Admin (Talk | contribs)

(diff) ← Older revision | Latest revision (diff) | Newer revision → (diff)
Jump to: navigation, search

In order to add new sensors the easiest way to do it is adding a new device through the device manager. The latest added sensors were implemented using it, and the old ones should be exported to this structure as makes the code more readable and extendable but unfortunately we didn't have time to do it yet. So if you want a fully-working device example have a look at simDev_Echo which is an example of use of the device manager, or ForceSensor and DredgeTool which are two different devices already implemented using the device manager.

You will need to fill a few classes and methods to declare how to parse the data, and create the appropriate classes to make the device work. This tutorial will try to explain the process from a data flow point of view, so it will start from the XML parser, go through the device itself and finally the publisher.

You can download the code of the example File:Newdevice.h and File:Newdevice.cpp.

Overview

Although it may seem difficult to add a new sensor it takes only a few changes besides creating the device itself. This tutorial tries to summarize the changes to use the device manager and create your own sensor or actuator. The following shceme is a good sum up to see the changes needed. Take into account some stages are not required depending on the kind of device your are creating.

Error creating thumbnail: Unable to save thumbnail to destination

Create the XML

First of all you will need to modify the dtd file ("/data/scenes/UWSimScene.dtd") to be able to configure your device in the XML config file, aka scene configuration file. If you don't know nothing about XML and DTD check this. This file is a "medium-large" file with the rules for the XML scene config files. You will need to add a new tag to the vehicle declaration, you have to search the declaration line and modify it. It should be similar to "<!ELEMENT vehicle (name, file, jointValues?, position?, orientation?, scaleFactor? ,virtualCamera*, ...". So just adding at the end a new tag with a proper name depending on your device should allow us to declare it inside a vehicle. If you add an "*" it will mean every vehicle can use as many devices as it needs, a "?" means device is optional and nothing means is required to add a vehicle.

After that you should state the different contents allowed in your sensor, usually a "name", "relativeTo", "position", "orientation" are required and depending on your sensor some others may be required or optional. So it is possible to declare a simple sensor editing the same file, adding something like this:

<!ELEMENT YOURDEVICE (name,relativeTo,position?,orientation?)>

In this case we have declared two required tags (name and relativeTo), and two optional. Every tag inside the new device is already defined in the DTD, so there is no need to redefine them, but new tags may be added the same way we defined the new device.

Create the barebone

Once it is done you should be able to add your sensor in an XML file and run it on UWSim, it will not complain as long as it follows the previous definition, but obviously it will ignore it (sorry it is not that easy). Now it is the right moment to create you device files, .h and .cpp should be enough for simple sensors. You should place this files in the proper folders ("include/uwsim/YOURDEVICE.h" and "src/YOURDEVICE.cpp") Something like this should be a good barebone:

The .h file should look like this:

#ifndef YOURDEVICE_ECHO_H_
#define YOURDEVICE_ECHO_H_
#include "SimulatedDevice.h"
#include <ros/ros.h>
using namespace uwsim;

/* You will need to add your code HERE */
class YOURDEVICE_Factory : public SimulatedDeviceFactory
{
public:
  //this is the only place the device/interface type is set
  YOURDEVICE_Factory(std::string type_ = "YOURDEVICE") :
      SimulatedDeviceFactory(type_)
  {
  }
  ;

  SimulatedDeviceConfig::Ptr processConfig(const xmlpp::Node* node, ConfigFile * config);
  bool applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration);
  std::vector<boost::shared_ptr<ROSInterface> > getInterface(ROSInterfaceInfo & rosInterface,
                                                            std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile);
};

#endif

The .cpp file:

#include <pluginlib/class_list_macros.h>
#include <uwsim/YOURDEVICE.h>

/* You will need to add your code HERE */

SimulatedDeviceConfig::Ptr YOURDEVICE_Factory::processConfig(const xmlpp::Node* node, ConfigFile * config){}

bool YOURDEVICE_Factory::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration){}

std::vector<boost::shared_ptr<ROSInterface> > YOURDEVICE_Factory::getInterface(ROSInterfaceInfo & rosInterface, std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile){}


#if ROS_VERSION_MINIMUM(1, 9, 0)
// new pluginlib API in Groovy and Hydro
PLUGINLIB_EXPORT_CLASS(FACTORYCLASSNAME_Factory, uwsim::SimulatedDeviceFactory)
#else
PLUGINLIB_REGISTER_CLASS(FACTORYCLASSNAME_Factory, FACTORYCLASSNAME_Factory, uwsim::SimulatedDeviceFactory)
#endif

After that you will need to add your factory class to the plugins xml. Just open "simdev_plugins.xml" and create a new "class" tag with the new device. Note: FACTORYCLASSNAME should be the same stated in the ".h" ".cpp" files and the name of the class that we will create later. The filled block should be like the others in the file, in our example:

<class name="YOURDEVICE_Factory" type="YOURDEVICE_Factory" 
base_class_type="uwsim::SimulatedDeviceFactory">
<description>
DESCRIPTION OF THE DEVICE
</description>
</class>

Then you will need to add these files to the CMakeLists.txt in order to build when you run UWSim. Just open CMakeLists.txt and find something like: "add_library(uwsim_plugins_simdev src/SimDev_Echo.cpp src/ForceSensor.cpp src/DredgeTool.cpp )" and add the new cpp to the list.

At this moment you should be able to build your new device (It still will not do anything, but it's recommended to check everything is going as it should).

Fill the classes

Now it is time to start filling the classes and make the device just added functional. The first recommended step, is to add the new device in the scene we are working so we can debug the code we are writing for the sensor. To do so just open an XML scene file (for instance default scene cirs.xml), and add the new device tag with its configuration. Depending on the parameters of the new sensor the content of it may be different but something like this is a starting point:

<YOURDEVICE>
  <name>DeviceName</name>
  <relativeTo>base_link</relativeTo>
  <position>
    <x>0</x>
    <y>0</y>
    <z>0</z>
  </position>  
  <orientation>
    <r>0</r>
    < p>0</p>
    <y>0</y>
  </orientation>
</YOURDEVICE>

Parsing arguments

Now if we run the simulator it will crash with a segmentation fault, so it is the right time to fill the classes to avoid it. The class we already created is a Factory class which will call different methods to parse and create the sensor but we will need to add a few more code to make it work. First class we should create is a parser class, this will be the one that receives the data from the XML, makes sure everything is correct and creates a logic structure that allows to build the sensor in the simulator. This class should just hold the data from the XML and will be the input for the device class. So here is a an example on how to create it, add this in the ".h" file:

#include "ConfigXMLParser.h"

class YOURDEVICE_Config : public SimulatedDeviceConfig
{
public:
  //XML members
  std::string relativeTo;
  double position[3], orientation[3];
  //constructor
  YOURDEVICE_Config(std::string type_) :
      SimulatedDeviceConfig(type_)
  {
  }
};

As can be seen we didn't create a variable for the "name" of the device. This is because SimulatedDeviceConfig class already has it and will parse it automatically, the name property of the device is required. Besides this relativeTo and position orientation variables have been created to hold the configuration values and check everything is OK. Now making use of this support class we can fill processConfig method from the factory class. Take into account we are using the libXML++ library to parse the XML files so we will receive a "const xmlpp::Node* node" with the device in it. To deal with this we have added the include so we can make use of some functions to get strings, doubles...etc that will make everything easier. So we should modify the previous defined method processConfig in the .cpp file to look like this:

SimulatedDeviceConfig::Ptr YOURDEVICE_Factory::processConfig(const xmlpp::Node* node, ConfigFile * config)
{
  YOURDEVICE_Config * cfg = new YOURDEVICE_Config(getType());
  xmlpp::Node::NodeList list = node->get_children();
  for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter)
  {
    const xmlpp::Node* child = dynamic_cast<const xmlpp::Node*>(*iter);
    if (child->get_name() == "relativeTo")
      config->extractStringChar(child, cfg->relativeTo);
    else if (child->get_name() == "position")
      config->extractPositionOrColor(child, cfg->position);
    else if (child->get_name() == "orientation")
      config->extractOrientation(child, cfg->orientation);
  }

  return SimulatedDeviceConfig::Ptr(cfg);
}

Now lets see what this function do. First of all we declare an object of the class we have just created to hold the configuration. Then we start looping the XML node received as argument. This node contains the information about our XML sensor so we start iterating through the list of his sons which will contain the different properties we assigned in the DTD (in this case relativeTo, position and orientation. Remember name is already parsed by the mother class) in the for loop. So in the loop we just use the config set of methods to extract the string from "relativeTo", the position array from "position" and orientation array from "orientation". So at the end of the for loop the object we created should be complete and ready to create a new device. Other methods that can be useful to parse arguments are:

void extractFloatChar(const xmlpp::Node* node, double &param);
void extractIntChar(const xmlpp::Node* node, int &param);
void extractUIntChar(const xmlpp::Node* node, unsigned int &param);
void extractStringChar(const xmlpp::Node* node, string &param);
void extractPositionOrColor(const xmlpp::Node* node, double param[3]);
void extractOrientation(const xmlpp::Node* node, double param[3]);

After this you should be able to successfully build and run the scene previously modified.

Creating the device

Now that we have finished parsing we will need a class that actually serves as device itself. In our example we will just create a simple class to hold the arguments and publish them in an interface we will create later. But this class is the one that should do the hard work. So this is an example of how to create the class, you can add it in the .h file:

class YOURDEVICE : public SimulatedDevice
{
public:
  std::string relativeTo;
  double position[3], orientation[3]; 

  YOURDEVICE(YOURDEVICE_Config * cfg);
};

As can be seen the only remarkable thing is the inheritance. And the constructor definition calls father's constructor using the config class that receives as argument. This allows to automatically relate everything using the class name.

YOURDEVICE::YOURDEVICE(YOURDEVICE_Config * cfg) :
    SimulatedDevice(cfg)
{
  this->relativeTo = cfg->relativeTo;
  this->position[0] = cfg->position[0];
  this->position[1] = cfg->position[1];
  this->position[2] = cfg->position[2];
  this->orientation[0] = cfg->orientation[0];
  this->orientation[1] = cfg->orientation[1];
  this->orientation[2] = cfg->orientation[2];
}

Now the only step missing is how to move from the config object to the device object. This is done through applyConfig method, this method is called iteratively until every device is properly configured or it reaches maximum iterations. The method receives 4 arguments, the AUV where the device is attached, the vehicle configuration, the scene configuration and the iteration. This information should be enough to create any kind of device needed (Physics Engine is called in a different step). So this is the example code to create the simple device previously defined.

bool NewDevice_Factory::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder, size_t iteration)
{
  if (iteration > 0)
    return true;
  for (size_t i = 0; i < vehicleChars.simulated_devices.size(); ++i)
    if (vehicleChars.simulated_devices[i]->getType() == this->getType())
    {
      YOURDEVICE_Config * cfg = dynamic_cast<YOURDEVICE_Config *>(vehicleChars.simulated_devices[i].get());
      if (cfg)
      {
        auv->devices->all.push_back(YOURDEVICE::Ptr(new YOURDEVICE(cfg)));
      }
      else
        OSG_FATAL << "YOURDEVICE device '" << vehicleChars.simulated_devices[i]->name << "' inside robot '"
            << vehicleChars.name << "' has empty cfg, discarding..." << std::endl;
    }
  return true;
}

As we can see this method iterates over all the simulated devices and if the device type matches, casts the config class and if everything went as it should and the config class is non-empty creates the device and adds it to the device factory. In this case we are not using any of the arguments provided by the function as our device does not have any purpose but depending on the device some of them will be necessary. For instance in this example we are trying to add our sensor relative to a part of the AUV, so we should locate this part and create an OSG node in our graph to represent this. So instead of just creating and pushing back the device (code inside "if (cfg)") we could do something like this:

int target=-1;
for(int j=0;j<auv->urdf->link.size();j++)
{
  if(auv->urdf->link[j]->getName()==cfg->relativeTo)
  {
    target=j;
  }
}
if(target==-1)
{
  OSG_FATAL << "YOURDEVICE device '" << vehicleChars.simulated_devices[i]->name << "' inside robot '"
          << vehicleChars.name << "' has an unknown relativeTo, discarding..." << std::endl;
}
else
{
  osg::ref_ptr < osg::Transform > vMd = (osg::Transform*) new osg::PositionAttitudeTransform;
  vMd->asPositionAttitudeTransform()->setPosition(osg::Vec3d(cfg->position[0], cfg->position[1], cfg->position[2]));
  vMd->asPositionAttitudeTransform()->setAttitude(
             osg::Quat(cfg->orientation[0], osg::Vec3d(1, 0, 0), cfg->orientation[1], osg::Vec3d(0, 1, 0),
             cfg->orientation[2], osg::Vec3d(0, 0, 1)));
  auv->urdf->link[target]->getParent(0)->getParent(0)->asGroup()->addChild(vMd);

  auv->devices->all.push_back(YOURDEVICE::Ptr(new YOURDEVICE(cfg,vMd)));
}

You may have noticed that now YOURDEVICE has a different interface. That is because now we don't need to keep all the cfg information, only the transform is needed as it is already part of the OSG graph as a son of the part where the device is attached. So anything needed can be attached to this node. Obviously the previous class and constructor needed a few changes:

class YOURDEVICE : public SimulatedDevice
{
public:
  osg::Node *parent;

  YOURDEVICE(NewDevice_Config * cfg,osg::Node *trackNode);
};
YOURDEVICE::YOURDEVICE(YOURDEVICE_Config * cfg, osg::Node *trackNode) :
    SimulatedDevice(cfg)
{
  this->parent=trackNode;
}

And some more includes will be needed to build the new class:

#include <osg/Node>
#include "SimulatedIAUV.h"
#include <osg/PositionAttitudeTransform>

So this should be enough to create our device, in the case the device interacts with the scene we should create the required nodes to do this under the OSG graph and let the engine do his stuff, or if we need an interface to publish something we will need to create a ROSInterface.

Creating a ROSInterface

There are mainly two kinds of devices, sensors and actuators. While actuators modify the scene graph, sensors measure some characteristic from the environment, for instance DredgeTool directly changes the environment(dredges sand) and ForceSensor measure the force applied in any part of the vehicle. For this reason some devices like ForceSensor need a ROSInterface in order to publish their measurement or result. In order to do so, we will need to create a publisher. This process is similar to the previous device creation, we will need to add a rule in the DTD file, fill the getInterface method and implement an interface. In this case we will not need to parse the information as ROS publisher will use name, topic and rate only (this should be enough for any kind of sensor).

So first step is edit the dtd file ("/data/scenes/UWSimScene.dtd"), look for "<!ELEMENT rosInterfaces ((ROSOdomToPAT*)?, (PATToROSOdom*)?, (WorldToROSTF*)?,...." and add a new tag on it, you need to use YOURDEVICEROS as name because the devicemanager relates sensors to interfaces through its name (devicename + ROS) (we should enhance this!). After that you should create a new rule for the sensor, such as:

<!ELEMENT YOURDEVICEROS (name, topic, rate?)>

Then we should add it to the XML so we can test it while developing. Take into account the name must be the same declared in the sensor. Just add the corresponding lines to the scene configuration file:

<YOURDEVICEROS>
  <name>DeviceName</name>
  <topic>g500/newdevice</topic>
  <rate>20</rate>
</YOURDEVICEROS>

Finally we will need to add the publisher class to our new sensor. In order to do so edit the .h file and add the following lines:

#include "ROSInterface.h"

class YOURDEVICE_ROSPublisher : public ROSPublisherInterface
{
  YOURDEVICE * dev;
public:
  YOURDEVICE_ROSPublisher(YOURDEVICE *dev, std::string topic, int rate) :
      ROSPublisherInterface(topic, rate), dev(dev)
  {
  }

  void createPublisher(ros::NodeHandle &nh);
  void publish();

  ~YOURDEVICE_ROSPublisher()
  {
  }
};

As can be seen the class is pretty simple, inherits from ROSPublisherInterface in order to create its own thread to publish the data and be independent from simulation steps. So the only thing to do is fill 3 methods, constructor, createPublisher and publish and we will have our publisher ready. After that we will still need to fill getInterface to create the link between the device and the publisher.

But, first things first, constructor is already completed as it copies a pointer to the device and calls ROSPublisherInterface constructor with topic name and rate. CreatePublisher and publish can be filled like this:

void YOURDEVICE_ROSPublisher::createPublisher(ros::NodeHandle &nh)
{
  ROS_INFO("YOURDEVICE_ROSPublisher on topic %s", topic.c_str());
  pub_ = nh.advertise < geometry_msgs::Pose > (topic, 1);
}
void YOURDEVICE_ROSPublisher::publish()
{
  geometry_msgs::Pose msg;
  boost::shared_ptr<osg::Matrix> mat = getWorldCoords(dev->parent);
  msg.position.x= mat->getTrans().x();
  msg.position.y= mat->getTrans().y();
  msg.position.z= mat->getTrans().z();
  msg.orientation.x=mat->getRotate().x();	
  msg.orientation.y=mat->getRotate().y();	
  msg.orientation.z=mat->getRotate().z();	
  msg.orientation.w=mat->getRotate().w();	
  pub_.publish(msg);
}

In this tutorial we made a publisher for geometry_msgs::pose that publishes to world to sensor position. So in createPublisher we just announces with ROS_INFO that our publisher has been created and is ready to publish and advertised the service in ros, take into account we use to variable pub_ for it. The publish method creates a message and it fills it with the position of the device using the getWorldCoords method from UWSimUtils with the node input we previously stored.

Our publisher is finished but it won't work as the device, publisher link is not ready yet. So we will have to modify the previously declared function "getInterface" inside the factory class. We can fill it with the following code:

std::vector<boost::shared_ptr<ROSInterface> > YOURDEVICE_Factory::getInterface(ROSInterfaceInfo & rosInterface, std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile)
{
  std::vector < boost::shared_ptr<ROSInterface> > ifaces;
  for (size_t i = 0; i < iauvFile.size(); ++i)
    for (size_t d = 0; d < iauvFile[i]->devices->all.size(); ++d)
      if (iauvFile[i]->devices->all[d]->getType() == this->getType()
          && iauvFile[i]->devices->all[d]->name == rosInterface.targetName)
      {
        ifaces.push_back(
            boost::shared_ptr < ROSInterface
                > (new YOURDEVICE_ROSPublisher(dynamic_cast<YOURDEVICE*>(iauvFile[i]->devices->all[d].get()),
                                                rosInterface.topic, rosInterface.rate)));
      }
  if (ifaces.size() == 0)
    ROS_WARN("Returning empty ROS interface for device %s...", rosInterface.targetName.c_str());
  return ifaces;
}

As can be seen this method only looks for suitable interfaces in the devices list and calls the constructor with the required parameters. But some other things could be checked if needed.

Finally we can safely run the example and we will see a new message for the new interface created:

 [ INFO] [1456156969.947795384]: YOURDEVICE_ROSPublisher on topic g500/test

Furthermore the topic published should contain the position of the part where we attached the device.

Share your work

Now that you have a new sensor working other people may be interested in using it. So if you think everything is fully working you can open a pull-request in our github, and if everything is ok we will add it to our core.