server.cpp

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Willow Garage, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Willow Garage, Inc. nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <rve_render_server/server.h>
#include <rve_render_server/billboard_text.h>
#include <rve_render_server/convert.h>
#include <rve_render_server/renderer.h>
#include <rve_render_server/render_window.h>
#include <rve_render_server/screen_rect.h>
#include <rve_render_server/scene.h>
#include <rve_render_server/camera.h>
#include <rve_render_server/material.h>
#include <rve_render_server/mesh_instance.h>
#include <rve_render_server/mesh_loader.h>
#include <rve_render_server/mesh.h>
#include <rve_render_server/texture.h>
#include <rve_render_server/texture_resource.h>
#include <rve_render_server/batching/points_manager.h>
#include <rve_render_server/batching/lines_manager.h>
#include <rve_render_server/batching/triangles_manager.h>
#include <rve_common/uuid.h>

#include <ros/ros.h>
#include <ros/callback_queue.h>

#include <rve_interfaces/BillboardText.h>
#include <rve_interfaces/Camera.h>
#include <rve_interfaces/RenderTarget.h>
#include <rve_interfaces/ScreenRect.h>
#include <rve_interfaces/Scene.h>
#include <rve_interfaces/Material.h>
#include <rve_interfaces/Texture.h>
#include <rve_interfaces/MeshInstance.h>
#include <rve_interfaces/Mesh.h>
#include <rve_interfaces/Sphere.h>
#include <rve_interfaces/Box.h>
#include <rve_interfaces/Cylinder.h>
#include <rve_interfaces/Cone.h>
#include <rve_interfaces/Points.h>
#include <rve_interfaces/Lines.h>
#include <rve_interfaces/Triangles.h>

#include <rve_interfaces/RenderTarget_pickRequest.h>
#include <rve_interfaces/RenderTarget_pickResponse.h>

#include <rve_rpc/call_handle.h>
#include <rve_rpc/server.h>

#include <OGRE/OgreSceneNode.h>

using namespace rve_common;

namespace rve_render_server
{

rve_render_server::Scene* lookupScene(Renderer* rend, const rve_common::UUID& id)
{
  rve_render_server::Scene* scene = rend->getScene(id);
  if (!scene)
  {
    throw std::runtime_error("Scene [" + UUID(id).toString() + "] does not exist");
  }

  return scene;
}

rve_render_server::Camera* lookupCamera(Renderer* rend, const rve_common::UUID& scene_id, const rve_common::UUID& id)
{
  rve_render_server::Scene* scene = lookupScene(rend, scene_id);
  rve_render_server::Camera* cam = scene->getCamera(id);
  if (!cam)
  {
    std::stringstream ss;
    ss << "Invalid camera [" << id << "]";
    throw std::runtime_error(ss.str());
  }

  return cam;
}

class CameraServer : public rve_interfaces::CameraServer
{
public:
  CameraServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::CameraServer(server)
  , renderer_(rend)
  {
  }

  rve_render_server::Camera* lookupCamera(const rve_common::UUID& scene_id, const rve_common::UUID& id)
  {
    return rve_render_server::lookupCamera(renderer_, scene_id, id);
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& camera_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->createCamera(camera_id);
  }

  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& camera_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->destroyCamera(camera_id);
  }

  virtual void setPosition(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::Vector3& pos)
  {
    lookupCamera(scene_id, id)->setPosition(fromRobot(pos));
  }

  virtual void setOrientation(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::Quaternion& orient)
  {
    lookupCamera(scene_id, id)->setOrientation(fromRobotRelative(orient));
  }

  virtual void setProjectionMatrix(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::Matrix4& projection_matrix)
  {
    lookupCamera(scene_id, id)->setProjectionMatrix(convert(projection_matrix));
  }


private:
  Renderer* renderer_;
};

class RenderTargetServer : public rve_interfaces::RenderTargetServer
{
public:
  RenderTargetServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::RenderTargetServer(server)
  , renderer_(rend)
  {
  }

  virtual void resize(const rve_msgs::UUID& id, uint32_t width, uint32_t height)
  {
    rve_render_server::RenderTarget* wnd = renderer_->getRenderTarget(id);
    wnd->resize(width, height);
  }

  virtual void attachCamera(const rve_msgs::UUID& id, const rve_msgs::UUID& camera_scene_id, const rve_msgs::UUID& camera_id)
  {
    rve_render_server::Camera* cam = lookupCamera(renderer_, camera_scene_id, camera_id);
    rve_render_server::RenderTarget* wnd = renderer_->getRenderTarget(id);
    wnd->attachCamera(cam);
    cam->addRenderTarget(wnd);
  }

  virtual void detachCamera(const rve_msgs::UUID& id)
  {
    rve_render_server::RenderTarget* wnd = renderer_->getRenderTarget(id);
    wnd->getCamera()->removeRenderTarget(wnd);
    wnd->attachCamera(0);
  }

  virtual void pick(pickHandle& handle)
  {
    const pickRequestConstPtr& req = handle.getRequest();
    uint32_t x0 = req->x0;
    uint32_t y0 = req->y0;
    uint32_t x1 = req->x1;
    uint32_t y1 = req->y1;
    const rve_common::UUID& id = req->id;
    rve_render_server::RenderTarget* wnd = renderer_->getRenderWindow(id);
    wnd->pick(x0, y0, x1, y1, boost::bind(&RenderTargetServer::pickCallback, this, _1, handle));
  }

  void pickCallback(V_PickRenderValues& vals, pickHandle& handle)
  {
    std::sort(vals.begin(), vals.end());
    V_Picked picked;
    renderer_->translatePickIDs(vals, picked);

    pickResponsePtr res(new pickResponse);
    V_Picked::iterator it = picked.begin();
    V_Picked::iterator end = picked.end();
    for (; it != end; ++it)
    {
      rve_msgs::PickResult pr;
      pr.id = it->object_id;
      pr.extra = it->object_extra;
      res->results.results.push_back(pr);
    }

    handle.respond(res);
  }

  virtual void requestRender(const rve_msgs::UUID& id)
  {
    rve_render_server::RenderTarget* target = renderer_->getRenderTarget(id);
    target->requestRender();
  }

  virtual void createWindow(const rve_msgs::UUID& id, const std::string& parent_window, uint32_t width, uint32_t height)
  {
    renderer_->createRenderWindow(id, parent_window, width, height);
  }

  virtual void createTexture(const rve_msgs::UUID& id, uint32_t width, uint32_t height)
  {
    renderer_->createRenderTexture(id, width, height);
  }

  virtual void destroy(const rve_msgs::UUID& id)
  {
    renderer_->destroyRenderTarget(id);
  }

private:
  Renderer* renderer_;
};

class SceneServer : public rve_interfaces::SceneServer
{
public:
  SceneServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::SceneServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& id)
  {
    rve_render_server::Scene* scene = renderer_->createScene(id);
    if (!scene)
    {
      throw std::runtime_error("Could not create scene [" + UUID(id).toString() + "]");
    }
  }

  virtual void destroy(const rve_msgs::UUID& id)
  {
    renderer_->destroyScene(id);
  }

private:
  Renderer* renderer_;
};

class PointsServer : public rve_interfaces::PointsServer
{
public:
  PointsServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::PointsServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& points_id, const rve_msgs::Points& points)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->getPointsManager()->add(points_id, points);
  }

  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& points_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->getPointsManager()->remove(points_id);
  }

private:
  Renderer* renderer_;
};

class LinesServer : public rve_interfaces::LinesServer
{
public:
  LinesServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::LinesServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& lines_id, const rve_msgs::Lines& lines)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->getLinesManager()->add(lines_id, lines);
  }

  virtual void destroyLines(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& lines_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);

    scene->getLinesManager()->remove(lines_id);
  }

private:
  Renderer* renderer_;
};

class TrianglesServer : public rve_interfaces::TrianglesServer
{
public:
  TrianglesServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::TrianglesServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& tris_id, const rve_msgs::Triangles& tris)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->getTrianglesManager()->add(tris_id, tris);
  }

  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& triangles_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);

    scene->getTrianglesManager()->remove(triangles_id);
  }

private:
  Renderer* renderer_;
};

class MaterialServer : public rve_interfaces::MaterialServer
{
public:
  MaterialServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::MaterialServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& id)
  {
    rve_render_server::MaterialPtr mat(new rve_render_server::Material(id));
    renderer_->addMaterial(id, mat);
  }

  virtual void destroy(const rve_msgs::UUID& id)
  {
    renderer_->removeMaterial(id);
  }

  virtual void setMaterial(const rve_msgs::UUID& id, const rve_msgs::Material& in_mat)
  {
    MaterialPtr mat = renderer_->getMaterial(id);
    if (!mat)
    {
      std::stringstream ss;
      ss << "Material [" << id << "] does not exist!";
      throw std::runtime_error(ss.str());
    }

    mat->setMaterial(in_mat);
  }

private:
  Renderer* renderer_;
};

class TextureServer : public rve_interfaces::TextureServer
{
public:
  TextureServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::TextureServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& id, uint32_t num_mip_maps)
  {
    rve_render_server::TexturePtr tex(new rve_render_server::Texture(id, num_mip_maps));
    renderer_->addTexture(id, tex);
  }

  virtual void destroy(const rve_msgs::UUID& id)
  {
    renderer_->removeTexture(id);
  }

  virtual void setImage(const rve_msgs::UUID& id, const sensor_msgs::Image& image)
  {
    TexturePtr tex = renderer_->getTexture(id);
    tex->setImage(image);
  }

private:
  Renderer* renderer_;
};

class MeshServer : public rve_interfaces::MeshServer
{
public:
  MeshServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::MeshServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& id, const rve_msgs::Mesh& mesh)
  {
    convertMesh(id, mesh);
  }

  virtual void destroy(const rve_msgs::UUID& id)
  {
    renderer_->removeMesh(id);
  }

private:
  Renderer* renderer_;
};

template<typename S>
class SimpleMeshObjectServer : public S
{
public:
  SimpleMeshObjectServer(Renderer* rend, rve_rpc::Server& server, const rve_common::UUID& mesh_id)
  : S(server)
  , renderer_(rend)
  , mesh_id_(mesh_id)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->createMeshInstance(inst_id, mesh_id_);
  }

  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->destroyMeshInstance(inst_id);
  }

  virtual void setMaterial(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, const rve_msgs::UUID& material_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    rve_render_server::MaterialPtr mat = renderer_->getMaterial(material_id);
    inst->setMaterial(mat);
  }

  virtual void setPosition(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, const rve_msgs::Vector3& pos)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    node->setPosition(fromRobot(pos));
  }

  virtual void setOrientation(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, const rve_msgs::Quaternion& orient)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    node->setOrientation(fromRobot(orient));
  }

  virtual void setScale(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, const rve_msgs::Vector3& scale)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    node->setScale(convert(scale));
  }

protected:
  Renderer* renderer_;

private:
  rve_common::UUID mesh_id_;
};

class MeshInstanceServer : public SimpleMeshObjectServer<rve_interfaces::MeshInstanceServer>
{
public:
  MeshInstanceServer(Renderer* rend, rve_rpc::Server& server)
  : SimpleMeshObjectServer<rve_interfaces::MeshInstanceServer>(rend, server, rve_common::UUID::Null)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, const rve_msgs::UUID& mesh_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->createMeshInstance(inst_id, mesh_id);
  }

  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->destroyMeshInstance(inst_id);
  }

  virtual void setMaterialForSubMesh(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, uint32_t submesh_index, const rve_msgs::UUID& material_id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);

    MaterialPtr material = renderer_->getMaterial(material_id);
    if (!material)
    {
      throw std::runtime_error("Material [" + UUID(material_id).toString() + "] does not exist");
    }

    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    if (!inst)
    {
      throw std::runtime_error("MeshInstance [" + UUID(material_id).toString() + "] does not exist");
    }

    inst->setMaterial(submesh_index, material);
  }
};

class SphereServer : public SimpleMeshObjectServer<rve_interfaces::SphereServer>
{
public:
  SphereServer(Renderer* rend, rve_rpc::Server& server)
  : SimpleMeshObjectServer<rve_interfaces::SphereServer>(rend, server, loadMesh("package://rve_render_server/media/models/sphere.dae")->getID())
  {
  }

  virtual void setRadius(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, float radius)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    node->setScale(Ogre::Vector3(radius, radius, radius));
  }
};

class BoxServer : public SimpleMeshObjectServer<rve_interfaces::BoxServer>
{
public:
  BoxServer(Renderer* rend, rve_rpc::Server& server)
  : SimpleMeshObjectServer<rve_interfaces::BoxServer>(rend, server, loadMesh("package://rve_render_server/media/models/cube.dae")->getID())
  {
  }
};

class CylinderServer : public SimpleMeshObjectServer<rve_interfaces::CylinderServer>
{
public:
  CylinderServer(Renderer* rend, rve_rpc::Server& server)
  : SimpleMeshObjectServer<rve_interfaces::CylinderServer>(rend, server, loadMesh("package://rve_render_server/media/models/cylinder.dae")->getID())
  {
  }

  virtual void setRadius(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, float radius)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    Ogre::Vector3 scale = node->getScale();
    scale.x = scale.y = radius;
    node->setScale(scale);
  }

  virtual void setHeight(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, float height)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    Ogre::Vector3 scale = node->getScale();
    scale.z = height;
    node->setScale(scale);
  }
};

class ConeServer : public SimpleMeshObjectServer<rve_interfaces::ConeServer>
{
public:
  ConeServer(Renderer* rend, rve_rpc::Server& server)
  : SimpleMeshObjectServer<rve_interfaces::ConeServer>(rend, server, loadMesh("package://rve_render_server/media/models/cone.dae")->getID())
  {
  }

  virtual void setRadius(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, float radius)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    Ogre::Vector3 scale = node->getScale();
    scale.x = scale.y = radius;
    node->setScale(scale);
  }

  virtual void setHeight(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& inst_id, float height)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::MeshInstance* inst = scene->getMeshInstance(inst_id);
    Ogre::SceneNode* node = inst->getSceneNode();
    Ogre::Vector3 scale = node->getScale();
    scale.z = height;
    node->setScale(scale);
  }
};

class ScreenRectServer : public rve_interfaces::ScreenRectServer
{
public:
  ScreenRectServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::ScreenRectServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, uint32_t zorder, float x0, float y0, float x1, float y1)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    target->createScreenRect(id, zorder, x0, y0, x1, y1);
  }

  virtual void destroy(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    target->destroyScreenRect(id);
  }

  virtual void setCorners(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, float x0, float y0, float x1, float y1)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    rve_render_server::ScreenRect* rect = target->getScreenRect(id);
    rect->setCorners(x0, y0, x1, y1);
  }

  virtual void setZOrder(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, uint32_t zorder)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    rve_render_server::ScreenRect* rect = target->getScreenRect(id);
    rect->setZOrder(zorder);
  }

  virtual void setColor(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, const rve_msgs::ColorRGB& color)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    rve_render_server::ScreenRect* rect = target->getScreenRect(id);
    Ogre::ColourValue c(color.r, color.g, color.b);
    rect->setColor(c);
  }

  virtual void setOpacity(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, float opacity)
  {
    //RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    //rve_render_server::ScreenRect* rect = target->getScreenRect(id);
    // TODO rect->setOpacity(opacity);
  }

  virtual void setTexture(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, const rve_msgs::UUID& texture_id)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    rve_render_server::ScreenRect* rect = target->getScreenRect(id);
    TexturePtr tex = renderer_->getTexture(texture_id);
    rect->setTexture(tex->getOgreTexture()->getName());
  }

  virtual void setTextureByResource(const rve_msgs::UUID& render_target_id, const rve_msgs::UUID& id, const std::string& resource_name)
  {
    RenderTarget* target = renderer_->getRenderTarget(render_target_id);
    rve_render_server::ScreenRect* rect = target->getScreenRect(id);

    std::string ogre_resource;
    if (lookupTextureResource(resource_name, ogre_resource))
    {
      rect->setTexture(ogre_resource);
    }
    else
    {
      throw std::runtime_error("Resource [" + resource_name + "] is not a loaded texture or render texture");
    }
  }

private:
  Renderer* renderer_;
};

class BillboardTextServer: public rve_interfaces::BillboardTextServer
{
public:
  BillboardTextServer(Renderer* rend, rve_rpc::Server& server)
  : rve_interfaces::BillboardTextServer(server)
  , renderer_(rend)
  {
  }

  virtual void create(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->createBillboardText(id);
  }
  virtual void destroy(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    scene->destroyBillboardText(id);
  }
  virtual void setPosition(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::Vector3& position)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::BillboardText* bbt = scene->getBillboardText(id);
    bbt->setPosition( position );
  }
  virtual void setText(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const std::string& text)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::BillboardText* bbt = scene->getBillboardText(id);
    bbt->setText( text );
  }
  virtual void setCharacterHeight(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, float height)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::BillboardText* bbt = scene->getBillboardText(id);
    bbt->setCharacterHeight(height);
  }
  virtual void setAlignment(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::TextAlignment& align)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::BillboardText* bbt = scene->getBillboardText(id);
    bbt->setAlignment(align);
  }
  virtual void setColor(const rve_msgs::UUID& scene_id, const rve_msgs::UUID& id, const rve_msgs::ColorRGB& color)
  {
    rve_render_server::Scene* scene = lookupScene(renderer_, scene_id);
    rve_render_server::BillboardText* bbt = scene->getBillboardText(id);
    bbt->setColor(color);
  }

private:
  Renderer* renderer_;
};

Server::Server(Renderer* renderer, const ros::NodeHandle& nh)
: renderer_(renderer)
, nh_(new ros::NodeHandle(nh))
{
  renderer_->registerInitFunction(boost::bind(&Server::init, this));
}

Server::~Server()
{
}

void Server::init()
{
  nh_->setCallbackQueue(renderer_->getServerThreadCallbackQueue());
  server_.reset(new rve_rpc::Server("render_server", *nh_));

  interfaces_.push_back(rve_interface_gen::InterfacePtr(new CameraServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new RenderTargetServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new ScreenRectServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new SceneServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new MaterialServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new TextureServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new MeshServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new MeshInstanceServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new SphereServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new BoxServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new CylinderServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new ConeServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new PointsServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new LinesServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new TrianglesServer(renderer_, *server_)));
  interfaces_.push_back(rve_interface_gen::InterfacePtr(new BillboardTextServer(renderer_, *server_)));

  server_->ready();
}

} // namespace rve_render_server