Real & Simulated Sensors

The Sensor Framework

Warning

This describes the sensor framework as it is intended to work, and not necessarily how all sensors are working at the moment.

The most abstract concepts of sensors in RobWork are the rw::sensor::Sensor and rwlibs::simulation::SimulatedSensor interfaces:

• SimulatedSensor: defined with a name, associated frame and an associated rw::sensor::SensorModel. The simulated sensor has an update function for stepping the simulated sensor forward in time. Changes are saved in the State variable. The same SimulatedSensor can be used in multiple simulators simultaneously, as SimualatedSensor is stateless. The SimulatedSensor interface makes is possible to get a separate stateful Sensor handle for each simulator. This makes it possible to use the same Sensor interface for both real and simulated sensors. The simulated sensor does still need to be stepped forward by a rwlibs::simulation::Simulator to produce meaningful output.

• Sensor: defined with a name, description, properties and an associated rw::sensor::SensorModel. The Sensor interface is to be considered a concrete instance of a sensor, that can be both a physical sensor or a SimulatedSensor instantiated in a specific Simulator instance.

• SensorModel: is an abstract representation defining one or more simulated or real sensors. The SensorModel is associated to a specific sensor frame in the WorkCell and has a name and description. It also defines data that is part of the RobWork State.

Example:

Consider a physical setup with a sensor. In a virtual environment, we then want to run four simultaneous simulations of the same setup in slightly different conditions.

First, we construct a rw::sensor::SensorModel to model the physical and virtual sensors with a general model of parameters and the data produced by a sensor modelled by this type of sensor. The data is to be stored in the State variable. The rw::sensor::Sensor interface is implemented to acquire data from the physical device, and we attach the SensorModel to the sensor to describe its parameters and output. Then we construct a rwlibs::simulation::SimulatedSensor and four rwlibs::simulation::Simulator instances. Each simulator can now run with the same SimulatedSensor, as the state of the SimulatedSensor is stored in the running State of each simulator. It is possible to wrap the SimulatedSensor under the Sensor interface to hide the distinction between real and physical sensors. Notice that the SimulatedSensor must still be stepped forward in the simulator to be able to retrieve information from it. To get the Sensor abstraction, the getHandle function can be used on SimulatedSensor. One Sensor handle is returned for each separate Simulator. To get the image from a SimulatedSensor for a specific State in the simulation, this can be retrieved from the SensorModel.

The Sensor interface is extended by more specific interfaces, for instance Camera, Scanner2D and Scanner25D. Equivalently the SensorModel is extended by more specific interfaces, such as CameraModel, Scanner2DModel, and Scanner25DModel.

The rwlibs::simulation namespace contains various sensors that can be simulated. This includes cameras, scanners that cary depth information, and a simulated Kinect sensor that can apply a noise model simulating a real Kinect sensor.

WorkCell Definition

A simulated sensor is defined in the WorkCell format by adding a certain property to the sensor frame. The property should have the name Camera, Scanner2D or Scanner25D to be recognised as a sensor frame in RobWorkStudio. An example is shown below. Notice that the property description does not influence the order of the numbers written inside the property tag. The numbers should always be written in the order shown.

<WorkCell name="WCName">
    ...
    <Frame name="Camera" refframe="WORLD" type="Movable">
        <Pos>0 0 2</Pos>
        <RPY />
        <Property name="Camera" desc="[fovy,width,height]">50 640 480</Property>
    </Frame>

    <Frame name="Scanner25D" refframe="WORLD" type="Movable">
        <Pos>0 0 2</Pos>
        <RPY />
        <Property name="Scanner25D" desc="[fovy,width,height]">50 640 480</Property>
    </Frame>

    <Frame name="Scanner2D" refframe="WORLD" type="Movable">
        <Pos>0 0 2</Pos>
        <RPY>90 0 0</RPY>
        <Property name="Scanner2D" desc="[fovy,height]">50 480</Property>
    </Frame>
    ...
</WorkCell>

Important!

• Multiple cameras are supported but only one camera property per frame!

• The width and height has no real dimension its the proportion between them that matters

• The camera looks in the negative Z-axis direction of the frame

• Field of view is in degree and is defined in the Y-axis

RobWorkStudio & The Sensor Plugin

RobWorkStudio will render the camera frustum for frames that has the Camera property. This is for instance shown in the following scene:

SensorTestScene scene from the Device & Scene Collection (RobWorkData).

You can change views between cameras using Ctrl + the key [1-9], were 1 is the default view.

To see the camera output, open the Sensors plugin in RobWorkStudio.

The sensor plugin in RobWorkStudio will look for frames in the WorkCell with the sensor property tag set. The plugin will use take the picture using OpenGL. This is done with the rwlibs::simulation::GLFrameGrabber. For Scanner2D and Scanner25D, the rwlibs::simulation::GLFrameGrabber25D is used.

Below, the camera view is shown for the SensorTestScene. This camera view is opened by selecting the camera in the dropdown in the sensor plugin, and clicking “Display Sensor”.

The sensor plugin to the right and the view after clicing “Display Sensor”

Notice that the camera picture will update automatically if you move the robot in the scene. The update speed can be changed in the plugin.

Code Examples

C++

#include <rw/core/PropertyMap.hpp>
#include <rw/core/Ptr.hpp>
#include <rw/kinematics/State.hpp>
#include <rw/loaders/WorkCellLoader.hpp>
#include <rwlibs/simulation/GLFrameGrabber.hpp>
#include <rwlibs/simulation/SimulatedCamera.hpp>
#include <rwslibs/rwstudioapp/RobWorkStudioApp.hpp>

using namespace rw::core;
using namespace rw::common;
using rw::graphics::SceneViewer;
using namespace rw::kinematics;
using rw::loaders::WorkCellLoader;
using rw::models::WorkCell;
using rw::sensor::Image;
using namespace rwlibs::simulation;
using namespace rws;

int main (int argc, char** argv)
{
    if (argc < 2) {
        std::cout << "Usage: " << argv[0] << "/path/to/robworkdata" << std::endl;
        exit (1);
    }

    static const std::string WC_FILE =
        std::string (argv[1]) + "/scenes/SensorTestScene/SimpleWorkcell.xml";
    const WorkCell::Ptr wc = WorkCellLoader::Factory::load (WC_FILE);
    if (wc.isNull ())
        RW_THROW ("WorkCell could not be loaded.");
    Frame* const camera = wc->findFrame ("Camera");
    if (camera == nullptr)
        RW_THROW ("Camera frame could not be found.");
    const PropertyMap& properties = camera->getPropertyMap ();
    if (!properties.has ("Camera"))
        RW_THROW ("Camera frame does not have Camera property.");
    const std::string parameters = properties.get< std::string > ("Camera");
    std::istringstream iss (parameters, std::istringstream::in);
    double fovy;
    int width;
    int height;
    iss >> fovy >> width >> height;
    std::cout << "Camera properties: fov " << fovy << " width " << width << " height " << height
              << std::endl;

    RobWorkStudioApp app ("");
    RWS_START (app)
    {
        RobWorkStudio* const rwstudio = app.getRobWorkStudio ();
        rwstudio->postOpenWorkCell (WC_FILE);
        TimerUtil::sleepMs (5000);

        const SceneViewer::Ptr gldrawer = rwstudio->getView ()->getSceneViewer ();
        const GLFrameGrabber::Ptr framegrabber =
            ownedPtr (new GLFrameGrabber (width, height, fovy));
        framegrabber->init (gldrawer);
        SimulatedCamera::Ptr simcam =
            ownedPtr (new SimulatedCamera ("SimulatedCamera", fovy, camera, framegrabber));
        simcam->setFrameRate (100);
        simcam->initialize ();
        simcam->start ();
        simcam->acquire ();

        static const double DT = 0.001;
        const Simulator::UpdateInfo info (DT);
        State state = wc->getDefaultState ();
        int cnt     = 0;
        const Image* img;
        while (!simcam->isImageReady ()) {
            std::cout << "Image is not ready yet. Iteration " << cnt << std::endl;
            simcam->update (info, state);
            cnt++;
        }
        img = simcam->getImage ();
        img->saveAsPPM ("Image1.ppm");
        simcam->acquire ();
        while (!simcam->isImageReady ()) {
            std::cout << "Image is not ready yet. Iteration " << cnt << std::endl;
            simcam->update (info, state);
            cnt++;
        }
        std::cout << "Took " << cnt << " steps" << std::endl;
        img = simcam->getImage ();
        std::cout << "Image: " << img->getWidth () << "x" << img->getHeight () << " bits "
                  << img->getBitsPerPixel () << " channels " << img->getNrOfChannels ()
                  << std::endl;
        img->saveAsPPM ("Image2.ppm");

        simcam->stop ();
        app.close ();
    }
    RWS_END ()

    return 0;
}

Python

from sdurw import *
from sdurw_simulation import *
from sdurw_loaders import *
from sdurws import *
import sys

if __name__ == '__main__':
    if len(sys.argv) < 2:
        raise Exception("Provide the path to RobWorkData as first argument.")    
    WC_FILE = str(sys.argv[1]) + "/scenes/SensorTestScene/SimpleWorkcell.xml"

    wc = WorkCellLoaderFactory.load(WC_FILE)
    if wc.isNull():
        raise Exception("WorkCell could not be loaded")
    camera = wc.findFrame("Camera")
    if camera is None:
        raise Exception("Camera frame could not be found.")
    properties = camera.getPropertyMap()
    if not properties.has("Camera"):
        raise Exception("Camera frame does not have Camera property.")
    parameters = properties.getString("Camera").split(" ")
    fovy = float(parameters[0])
    width = int(parameters[1])
    height = int(parameters[2])
    print("Camera properties: fov " + str(fovy) + " width " + str(width) + " height " + str(height));

    rwstudio = getRobWorkStudioInstance();
    rwstudio.postOpenWorkCell(WC_FILE);
    sleep(5);
    gldrawer = rwstudio.getView().getSceneViewer();
    framegrabber = sdurw_simulation.ownedPtr( GLFrameGrabber(width,height,fovy) );
    framegrabber.init(gldrawer);
    simcam = SimulatedCamera("SimulatedCamera", fovy, camera, framegrabber.asFrameGrabberPtr());
    simcam.setFrameRate(100);
    simcam.initialize();
    simcam.start();
    simcam.acquire();

    DT = 0.001;
    info = UpdateInfo(DT);
    state = wc.getDefaultState();
    cnt = 0;
    while not simcam.isImageReady():
        print("Image is not ready yet. Iteration " + str(cnt));
        simcam.update(info, state);
        cnt = cnt+1;
    img = simcam.getImage();
    img.saveAsPPM("Image1.ppm");
    simcam.acquire();
    while not simcam.isImageReady():
        print("Image is not ready yet. Iteration " + str(cnt));
        simcam.update(info, state);
        cnt = cnt+1;
    print("Took " + str(cnt) + " steps");
    img = simcam.getImage();
    print("Image: " + str(img.getWidth()) + "x" + str(img.getHeight()) + " bits " + str(img.getBitsPerPixel()) + " channels " + str(img.getNrOfChannels()));
    img.saveAsPPM("Image2.ppm");

    simcam.stop()
    closeRobWorkStudio()
    sleep(10)

    print("Example-Done")

Java

import java.lang.String;

import org.robwork.*;
import org.robwork.sdurw.*;
import org.robwork.sdurw_simulation.*;
import org.robwork.sdurws.RobWorkStudioPtr;
import static org.robwork.sdurw_simulation.sdurw_simulation.ownedPtr;
import static org.robwork.sdurws.sdurws.getRobWorkStudioInstance;

public class SimulatedCameraExample {
    public static void main(String[] args) throws Exception {
        LoaderRW.load("sdurw");
        LoaderRW.load("sdurw_simulation");
        LoaderRWS.load();
        
        if (args.length != 1)
        	throw new Exception("Provide the path to RobWorkData as first argument.");
        
        final String WC_FILE =
                args[0] + "/scenes/SensorTestScene/SimpleWorkcell.xml";

        WorkCellPtr wc = WorkCellLoaderFactory.load(WC_FILE);
        if (wc.isNull())
            throw new Exception("WorkCell could not be loaded.");
        Frame camera = wc.findFrame("Camera");
        if (camera == null)
            throw new Exception("Camera frame could not be found.");
        PropertyMap properties = camera.getPropertyMap();
        if (!properties.has("Camera"))
            throw new Exception("Camera frame does not have Camera property.");
        String[] parameters = properties.getString("Camera").split(" ");
        Double fovy = Double.parseDouble(parameters[0]);
        Integer width = Integer.parseInt(parameters[1]);
        Integer height = Integer.parseInt(parameters[2]);
        System.out.print("Camera properties: fov " + fovy);
        System.out.println(" width " + width + " height " + height);

        RobWorkStudioPtr rwstudio = getRobWorkStudioInstance();
        rwstudio.postOpenWorkCell(WC_FILE);
        Thread.sleep(5000);
        SceneViewerPtr gldrawer = rwstudio.getView().getSceneViewer();
        GLFrameGrabberPtr framegrabber =
        		ownedPtr( new GLFrameGrabber(width,height,fovy) );
        framegrabber.init(gldrawer);
        SimulatedCamera simcam = new SimulatedCamera("SimulatedCamera",
        		fovy, camera, framegrabber.asFrameGrabberPtr());
        simcam.setFrameRate(100);
        simcam.initialize();
        simcam.start();
        simcam.acquire();

        final double DT = 0.001;
        Simulator.UpdateInfo info = new Simulator.UpdateInfo(DT);
        State state = wc.getDefaultState();
        int cnt = 0;
        Image img;
        while (!simcam.isImageReady()) {
            System.out.println("Image is not ready yet. Iteration " + cnt);
            simcam.update(info, state);
            cnt++;
        }
        img = simcam.getImage();
        img.saveAsPPM("Image1.ppm");
        simcam.acquire();
        while (!simcam.isImageReady()) {
            System.out.println("Image is not ready yet. Iteration " + cnt);
            simcam.update(info, state);
            cnt++;
        }
        System.out.println("Took " + cnt + " steps");
        img = simcam.getImage();
        System.out.print("Image: " + img.getWidth() + "x" + img.getHeight());
        System.out.print(" bits " + img.getBitsPerPixel());
        System.out.println(" channels " + img.getNrOfChannels());
        img.saveAsPPM("Image2.ppm");

        simcam.stop();
        rwstudio.postExit();
        Thread.sleep(1000);
    }
}

LUA

require("sdurw_core")
require("sdurw_kinematics")
require("sdurw_models")
require("sdurw_sensor")
require("sdurw")
require("sdurw_simulation")
require("sdurw_loaders")
require("sdurws")

using("sdurw_core")
using("sdurw_kinematics")
using("sdurw")
using("sdurw_simulation")
using("sdurw_loaders")
using("sdurws")

if #arg < 1 then
    error("Provide the path to RobWorkData as first argument.")
end

local WC_FILE = arg[1] .. "/scenes/SensorTestScene/SimpleWorkcell.xml"
print(WC_FILE)

local wc = WorkCellLoaderFactory.load(WC_FILE)
if wc:isNull() then
    error("WorkCell could not be loaded")
end
local camera = wc:findFrame("Camera")
if camera == nil then
    error("Camera frame could not be found.")
end
local properties = camera:getPropertyMap();
if not properties:has("Camera") then
    error("Camera frame does not have Camera property.")
end
local parameters = properties:getString("Camera");
local parlist={}
for str in string.gmatch(parameters, "([^%s]+)") do
    table.insert(parlist, str)
end
fovy = tonumber(parlist[1])
width = tonumber(parlist[2])
height = tonumber(parlist[3])
print("Camera properties: fov " .. tostring(fovy) .. " width " .. tostring(width) .. " height " .. tostring(height));

local rwstudio = getRobWorkStudioInstance();
rwstudio:postOpenWorkCell(WC_FILE);
sleep(2);
local gldrawer = rwstudio:getView():getSceneViewer();
local framegrabber = sdurw_simulation.ownedPtr( GLFrameGrabber(width,height,fovy) );
framegrabber:init(gldrawer);
local simcam = SimulatedCamera("SimulatedCamera", fovy, camera, framegrabber:asFrameGrabberPtr());
simcam:setFrameRate(100);
simcam:initialize();
simcam:start();
simcam:acquire();

local DT = 0.001;
local info = UpdateInfo(DT);
local state = wc:getDefaultState();
local cnt = 0;
local img;
while not simcam:isImageReady() do
    print("Image is not ready yet. Iteration " .. tostring(cnt));
    simcam:update(info, state);
    cnt = cnt+1;
end
img = simcam:getImage();
img:saveAsPPM("Image1.ppm");
simcam:acquire();
while not simcam:isImageReady() do
    print("Image is not ready yet. Iteration " .. tostring(cnt));
    simcam:update(info, state);
    cnt = cnt+1;
end
print("Took " .. tostring(cnt) .. " steps");
img = simcam:getImage();
print("Image: " .. tostring(img:getWidth()) .. "x" .. tostring(img:getHeight()) .. " bits " .. tostring(img:getBitsPerPixel()) .. " channels " .. tostring(img:getNrOfChannels()));
img:saveAsPPM("Image2.ppm");

simcam:stop();
sleep(20);
rwstudio:postExit();

while(isRunning()) do
    sleep(1);
end
sleep(10);
print("Example-Done");