#!/usr/bin/env python

# Authors: see AUTHORS.md at project root.
# CopyPolicy: released under the terms of the LGPLv2.1, see LICENSE at project root.
# URL: https://github.com/jgvictores/master-ipr

# Dependencies:
# - https://github.com/roboticslab-uc3m/openrave-yarp-plugins  (provides OpenraveYarpPluginLoader, YarpOpenraveRobotManager, YarpOpenraveRGBDSensor)
# - https://github.com/asrob-uc3m/yarp-devices  (provides RobotServer)
#
# Also counts on `/usr/local/share/master-ipr-openrave-models/contexts/openrave/ecro/mapping_room.env.xml` model installed from this repo

import openravepy
from openravepy import *

try:
    RaveInitialize()

    if not RaveLoadPlugin('OpenraveYarpPluginLoader'):
        raveLogError("Plugin not correctly loaded")

    env=Environment()
    env.SetViewer('qtcoin')
    env.Load('/usr/local/share/master-ipr/contexts/openrave/map11.env.xml')

    OpenraveYarpPluginLoader = RaveCreateModule(env,'OpenraveYarpPluginLoader')

    print OpenraveYarpPluginLoader.SendCommand('open --device RobotServer --subdevice YarpOpenraveRobotManager --robotIndex 0 --physics ode')

    print OpenraveYarpPluginLoader.SendCommand('open --device RGBDSensorWrapper --subdevice YarpOpenraveRGBDSensor --robotIndex 0 --depthSensorIndex 0')  # here not using --cameraSensorIndex 0

    while 1:
        pass

finally:
    RaveDestroy()
