test_playpen.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_client_common/client_context.h"
#include "rve_render_server/init.h"
#include "rve_render_client/init.h"
#include "rve_render_client/render_window.h"
#include "rve_render_client/render_texture.h"
#include "rve_render_client/screen_rect.h"
#include "rve_render_client/scene.h"
#include "rve_render_client/camera.h"
#include "rve_render_client/material.h"
#include "rve_render_client/mesh_instance.h"
#include "rve_render_client/sphere.h"
#include "rve_render_client/box.h"
#include "rve_render_client/cylinder.h"
#include "rve_render_client/cone.h"
#include "rve_render_client/points.h"
#include "rve_render_client/lines.h"
#include "rve_render_client/triangles.h"


#include <rve_wx/app.h>
#include <rve_wx/render_window.h>

#include <rve_msgs/Points.h>
#include <rve_msgs/Lines.h>
#include <rve_msgs/Triangles.h>
#include <rve_msgs/Mesh.h>

#include <rve_msgs/make_vector3.h>
#include <rve_msgs/make_quaternion.h>

#include <Eigen/Geometry>

#include <ros/package.h>
#include <ros/time.h>

#include <wx/wx.h>

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

using namespace rve_client_common;
using namespace rve_render_client;

class MyFrame : public wxFrame
{
public:
  MyFrame(const ClientContextPtr& context, wxWindow* parent)
  : wxFrame(parent, -1, _("test_playpen"), wxDefaultPosition, wxSize(1024, 768), wxDEFAULT_FRAME_STYLE)
  , private_nh_("~")
  , left_mouse_down_( false )
  , middle_mouse_down_( false )
  , right_mouse_down_( false )
  , mouse_x_( 0 )
  , mouse_y_( 0 )
  , timer_(this)
  {
    context_ = context;
    rve_wx::RenderWindow* rw = new rve_wx::RenderWindow(context.get(), this, wxID_ANY);
    render_window_ = rw;

    scene_ = rve_render_client::createScene(context_);
    camera_ = rve_render_client::createCamera(scene_.get());
    camera_->setPosition(rve_msgs::makeVector3(-10, 0, 0));
    camera_->lookAt(rve_msgs::makeVector3(0, 0, 0));
    camera_->setAutoAspectRatio(true);
    render_window_->getRemoteRenderWindow()->attachCamera(camera_);

#if 01
    MeshInstancePtr inst = createMeshInstance(scene_.get(), "package://pr2_description/meshes/base_v0/base.dae");
    inst->setPosition(rve_msgs::makeVector3(0.0, 0.0, 2.0));
    scene_objects_.push_back(inst);

#if 01
    SpherePtr sphere = createSphere(scene_.get());
    MaterialPtr mat = createMaterial(context_.get());
    sphere->setMaterial(mat);
    mat->setDiffuse(Color(1.0, 0.0, 0.0));

    scene_objects_.push_back(sphere);
    materials_.push_back(mat);

    BoxPtr box = createBox(scene_.get());
    mat = createMaterial(context_.get());
    mat->setDiffuse(Color(0.0, 1.0, 0.0));
    box->setMaterial(mat);
    box->setPosition(rve_msgs::makeVector3(0, 2, 0));
    box->setScale(rve_msgs::makeVector3(1.0, 0.5, 2.0));
    scene_objects_.push_back(box);
    materials_.push_back(mat);

    CylinderPtr cyl = createCylinder(scene_.get());
    mat = createMaterial(context_.get());
    mat->setDiffuse(Color(0.0, 0.0, 1.0));
    cyl->setMaterial(mat);
    cyl->setPosition(rve_msgs::makeVector3(0, -2, 0));
    cyl->setRadius(0.2);
    cyl->setHeight(2.0);
    scene_objects_.push_back(cyl);
    materials_.push_back(mat);

    ConePtr cone = createCone(scene_.get());
    mat = createMaterial(context_.get());
    mat->setDiffuse(Color(1.0, 1.0, 1.0));
    cone->setMaterial(mat);
    cone->setPosition(rve_msgs::makeVector3(0, -4, 0));
    cone->setRadius(0.2);
    cone->setHeight(2.0);
    scene_objects_.push_back(cone);
    materials_.push_back(mat);
#endif
#endif

#if 0
    {
      rve_msgs::PointsPtr points(new rve_msgs::Points);
      points->type = rve_msgs::Points::TYPE_BILLBOARDS;
      points->scale.x = 0.05;
      points->scale.y = 0.05;
      points->scale.z = 0.05;

      for (uint32_t x = 0; x < 100; ++x)
      {
        for (uint32_t y = 0; y < 100; ++y)
        {
          for (uint32_t z = 0; z < 10; ++z)
          {
            rve_msgs::Vector3 pos;
            pos.x = -10.0 - x*0.1;
            pos.y = y*0.1;
            pos.z = z*0.1;
            points->positions.push_back(pos);

            std_msgs::ColorRGBA col;
            col.r = x * 0.1;
            col.g = y * 0.1;
            col.b = z * 0.1;
            col.a = 1.0;//(z % 2 == 0) ? 1.0 : 0.5;
            points->colors.push_back(col);
          }
        }
      }

      PointsPtr p = createPoints(scene_.get(), points);
      scene_objects_.push_back(p);
    }
#endif

#if 0
    {
      rve_msgs::LinesPtr lines(new rve_msgs::Lines);
      lines->type = rve_msgs::Lines::TYPE_BILLBOARD_LIST;
      lines->scale.x = 0.05;
      lines->scale.y = 0.05;
      lines->scale.z = 0.05;

      for (uint32_t x = 0; x < 10; ++x)
      {
        for (uint32_t y = 0; y < 10; ++y)
        {
          for (uint32_t z = 0; z <= 10; z += 10)
          {
            rve_msgs::Vector3 pos;
            pos.x = -10.0 - x*0.1;
            pos.y = y*0.1;
            pos.z = z*0.1;
            lines->positions.push_back(pos);

            std_msgs::ColorRGBA col;
            col.r = x * 0.1;
            col.g = y * 0.1;
            col.b = z * 0.1;
            col.a = 1.0;//(z % 2 == 0) ? 1.0 : 0.5;
            lines->colors.push_back(col);
          }
        }
      }

      LinesPtr l = createLines(scene_.get(), lines);
      scene_objects_.push_back(l);
    }
#endif

#if 0
    {
      rve_msgs::TrianglesPtr tris(new rve_msgs::Triangles);

      Eigen::Vector3f offsets[3] =
      {
        Eigen::Vector3f(0.0, 0.0, 0.0),
        Eigen::Vector3f(0.0, 0.0, 0.1),
        Eigen::Vector3f(0.1, 0.0, 0.0)
      };

      for (uint32_t x = 0; x < 10; ++x)
      {
        for (uint32_t y = 0; y < 10; ++y)
        {
          for (uint32_t z = 0; z < 10; ++z)
          {
            for (uint32_t j = 0; j < 3; ++j)
            {
              rve_msgs::Vector3 pos;
              pos.x = -10.0 - x*0.1 + offsets[j].x();
              pos.y = y*0.1 + offsets[j].y();
              pos.z = z*0.1 + offsets[j].z();
              tris->positions.push_back(pos);

              std_msgs::ColorRGBA col;
              col.r = x * 0.1;
              col.g = y * 0.1;
              col.b = z * 0.1;
              col.a = 1.0;//(z % 2 == 0) ? 1.0 : 0.5;
              tris->colors.push_back(col);
            }
          }
        }
      }

      TrianglesPtr t = createTriangles(scene_.get(), tris);
      scene_objects_.push_back(t);
    }
#endif


#if 01
    rtt_scene_ = createScene(context_);
    rtt_camera_ = createCamera(rtt_scene_.get());
    rtt_camera_->setPosition(rve_msgs::makeVector3(-2, 0, 0));
    rtt_camera_->lookAt(rve_msgs::makeVector3(0, 0, 0));
    rtt_camera_->setAutoAspectRatio(true);
    rtt_ = createRenderTexture(context_.get(), 500, 500);
    rtt_->attachCamera(rtt_camera_);

    scene_objects_.push_back(rve_render_client::createMeshInstance(rtt_scene_.get(), "package://rve_render_server/media/models/sphere.dae"));
#endif

    {
      rve_msgs::MeshPtr mesh(new rve_msgs::Mesh);
      mesh->submeshes.resize(1);

      rve_msgs::SubMesh& sm = mesh->submeshes[0];
      sm.positions.push_back(rve_msgs::makeVector3(0.0, -0.5, 0.5));
      sm.positions.push_back(rve_msgs::makeVector3(0.0, 0.5, 0.5));
      sm.positions.push_back(rve_msgs::makeVector3(0.0, -0.5, -0.5));
      sm.positions.push_back(rve_msgs::makeVector3(0.0, 0.5, -0.5));
      sm.indices.push_back(0);
      sm.indices.push_back(1);
      sm.indices.push_back(2);
      sm.indices.push_back(2);
      sm.indices.push_back(1);
      sm.indices.push_back(3);

      sm.normals.push_back(rve_msgs::makeVector3(-1.0, 0.0, 0.0));
      sm.normals.push_back(rve_msgs::makeVector3(-1.0, 0.0, 0.0));
      sm.normals.push_back(rve_msgs::makeVector3(-1.0, 0.0, 0.0));
      sm.normals.push_back(rve_msgs::makeVector3(-1.0, 0.0, 0.0));

      sm.tex_coords.resize(1);
      rve_msgs::SubMesh::_tex_coords_type& tc = sm.tex_coords;
      tc[0].dims = 2;
      tc[0].array.resize(4);
      tc[0].array[0].uvw[0] = 1.0;
      tc[0].array[0].uvw[1] = 0.0;

      tc[0].array[1].uvw[0] = 0.0;
      tc[0].array[1].uvw[1] = 0.0;

      tc[0].array[2].uvw[0] = 1.0;
      tc[0].array[2].uvw[1] = 1.0;

      tc[0].array[3].uvw[0] = 0.0;
      tc[0].array[3].uvw[1] = 1.0;

      sm.material_index = 0;

      mesh->materials.resize(1);
      rve_msgs::Material& mat = mesh->materials[0];
      mat.id = rve_common::UUID::Generate();
      mat.opacity = 1.0;
      mat.has_texture = true;
      mat.texture = "rtt://" + rtt_->getID().toString();
      mat.disable_shading = true;
      mat.cull_mode = rve_msgs::Material::CULL_NONE;
      //mat.texture = "package://rve_render_server/media/textures/no_image.png";

      MeshPtr m = createMesh(context_.get(), mesh);
      MeshInstancePtr inst = createMeshInstance(scene_.get(), m);
      inst->setPosition(rve_msgs::makeVector3(0.0, 2.0, 2.0));
      scene_objects_.push_back(inst);
    }

    {
      ScreenRectPtr rect = render_window_->getRemoteRenderWindow()->createScreenRect(0, 0.8f, 0.8f, 1.0f, 1.0f);
      rect->setTexture("rtt://" + rtt_->getID().toString());
      rect->setOpacity(0.5);
      screen_rect_ = rect;
    }

    Connect(wxEVT_TIMER, wxTimerEventHandler(MyFrame::onTimer));

    render_window_->Connect( wxEVT_LEFT_DOWN, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_MIDDLE_DOWN, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_RIGHT_DOWN, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_MOTION, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_LEFT_UP, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_MIDDLE_UP, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_RIGHT_UP, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );
    render_window_->Connect( wxEVT_MOUSEWHEEL, wxMouseEventHandler( MyFrame::onMouseEvents ), NULL, this );

    timer_.Start(16);
  }

  ~MyFrame()
  {
  }

  void onTimer(wxTimerEvent& evt)
  {
#if 0
    if (last_update_.isZero())
    {
      last_update_ = ros::WallTime::now();
      return;
    }

    ros::WallTime cur = ros::WallTime::now();
    ros::WallDuration diff = cur - last_update_;
    last_update_ = cur;
    double dt = diff.toSec();
#endif
  }

  void onMouseEvents( wxMouseEvent& event )
  {
    int lastX = mouse_x_;
    int lastY = mouse_y_;

    mouse_x_ = event.GetX();
    mouse_y_ = event.GetY();

    if ( event.LeftDown() )
    {
     left_mouse_down_ = true;
     middle_mouse_down_ = false;
     right_mouse_down_ = false;
    }
    else if ( event.MiddleDown() )
    {
     left_mouse_down_ = false;
     middle_mouse_down_ = true;
     right_mouse_down_ = false;
    }
    else if ( event.RightDown() )
    {
     left_mouse_down_ = false;
     middle_mouse_down_ = false;
     right_mouse_down_ = true;

#if 0
      if (context_->isConnected())
      {
        context_->close();
      }
      else
      {
        context_->reopen();
      }
#endif
    }
    else if ( event.LeftUp() )
    {
     left_mouse_down_ = false;
    }
    else if ( event.MiddleUp() )
    {
     middle_mouse_down_ = false;
    }
    else if ( event.RightUp() )
    {
     right_mouse_down_ = false;
    }
    else if ( event.Dragging() )
    {
     int32_t diff_x = mouse_x_ - lastX;
     int32_t diff_y = mouse_y_ - lastY;

     bool handled = false;
     if ( left_mouse_down_ )
     {
       Eigen::Quaternionf quat(Eigen::AngleAxisf(diff_y * 0.002, Eigen::Vector3f::UnitY()));
       Eigen::Quaternionf quat2(Eigen::AngleAxisf(-diff_x * 0.002, Eigen::Vector3f::UnitZ()));
       Eigen::Quaternionf combined = quat * quat2;
       camera_->rotate(rve_msgs::makeQuaternion(combined.x(), combined.y(), combined.z(), combined.w()));

       handled = true;
     }
     else if ( middle_mouse_down_ )
     {
       camera_->moveRelative(Eigen::Vector3f(0.0, -diff_x * 0.01, -diff_y * 0.01));

       handled = true;
     }
     else if ( right_mouse_down_ )
     {
       camera_->moveRelative(Eigen::Vector3f(-diff_y * 0.01, 0.0, 0.0));

       handled = true;
     }
    }

    if ( event.GetWheelRotation() != 0 )
    {
     //camera_->scrollWheel( event.GetWheelRotation(), event.CmdDown(), event.AltDown(), event.ShiftDown() );
    }

    if (!left_mouse_down_ && !right_mouse_down_ && !middle_mouse_down_)
    {
      render_window_->getRemoteRenderWindow()->pick(mouse_x_, mouse_y_);
    }
  }

private:
  ros::NodeHandle private_nh_;
  rve_wx::RenderWindow* render_window_;

  ClientContextPtr context_;
  ScenePtr scene_;
  CameraPtr camera_;

  ScenePtr rtt_scene_;
  CameraPtr rtt_camera_;
  RenderTexturePtr rtt_;

  ScreenRectPtr screen_rect_;

  std::vector<SceneObjectPtr> scene_objects_;
  std::vector<MaterialPtr> materials_;

  // Mouse handling
  bool left_mouse_down_;
  bool middle_mouse_down_;
  bool right_mouse_down_;
  int mouse_x_;
  int mouse_y_;

  wxTimer timer_;

  ros::WallTime last_update_;
};

// our normal wxApp-derived class, as usual
class MyApp : public rve_wx::App
{
public:
  MyApp(const ClientContextPtr& context)
  : context_(context)
  {}

  bool OnInit()
  {
    if (!rve_wx::App::OnInit())
    {
      return false;
    }

    wxFrame* frame = new MyFrame(context_, NULL);
    SetTopWindow(frame);
    frame->Show();
    return true;
  }

private:
  ClientContextPtr context_;
};

DECLARE_APP(MyApp);

int main(int argc, char** argv)
{
  try
  {
    ros::init(argc, argv, "test_playpen", ros::init_options::NoSigintHandler);
    rve_render_server::init(true, "render_server");
    rve_render_client::init();
    ros::AsyncSpinner as(0);
    as.start();

    ClientContextPtr context = createContext("render_server", ros::NodeHandle("render_server"));
    wxApp* app = new MyApp(context);
    wxApp::SetInstance(app);
    wxEntry(argc, argv);

    rve_render_client::shutdown();
    rve_render_server::shutdown();
  }
  catch (std::exception& e)
  {
    ROS_ERROR("Caught exception: %s", e.what());
  }
}