Add types for lidar visual

Changelog.md

@@ -6,6 +6,9 @@
 
 ### Ignition Rendering 4.0.0
 
+1. Added Lidar Visual Types for Ogre1
+    * [Pull request #114](https://github.com/ignitionrobotics/ign-rendering/pull/114)
+
 1. Added Lidar Visualisation for Ogre1
     * [Pull request #103](https://github.com/ignitionrobotics/ign-rendering/pull/103)
 

examples/lidar_visual/GlutWindow.cc

@@ -41,6 +41,10 @@
 #include <ignition/rendering/OrbitViewController.hh>
 #include <ignition/rendering/RayQuery.hh>
 #include <ignition/rendering/Scene.hh>
+#include <ignition/rendering.hh>
+
+using namespace ignition;
+using namespace rendering;
 
 #include "GlutWindow.hh"
 #include "example_config.hh"
@@ -59,6 +63,16 @@ ir::ImagePtr g_image;
 
 bool g_initContext = false;
 
+std::vector<double> g_lidarData;
+ir::LidarVisualPtr g_lidar;
+bool g_lidarVisualUpdateDirty = false;
+bool g_showNonHitting = true;
+LidarVisualType g_lidarVisType = LidarVisualType::LVT_TRIANGLE_STRIPS;
+
+std::chrono::steady_clock::duration g_time{0};
+std::chrono::steady_clock::time_point g_startTime;
+double prevUpdateTime;
+
 #if __APPLE__
   CGLContextObj g_context;
   CGLContextObj g_glutContext;
@@ -227,6 +241,48 @@ void handleMouse()
   }
 }
 
+//////////////////////////////////////////////////
+void updateLidarVisual()
+{
+  g_startTime = std::chrono::steady_clock::now();
+
+  // change detected due to key press
+  if (g_lidarVisualUpdateDirty)
+  {
+    g_lidar->SetDisplayNonHitting(g_showNonHitting);
+    g_lidar->SetPoints(g_lidarData);
+    g_lidar->SetType(g_lidarVisType);
+    g_lidar->Update();
+    g_lidarVisualUpdateDirty = false;
+
+    g_time = std::chrono::steady_clock::now() - g_startTime;
+    prevUpdateTime = std::chrono::duration_cast<std::chrono::microseconds>(
+                                                            g_time).count();
+  }
+}
+
+
+//////////////////////////////////////////////////
+void drawText(int _x, int _y, const std::string &_text)
+{
+  glMatrixMode(GL_PROJECTION);
+  glPushMatrix();
+  glLoadIdentity();
+  gluOrtho2D(0.0, imgw, 0.0, imgh);
+  glMatrixMode(GL_MODELVIEW);
+  glPushMatrix();
+  glLoadIdentity();
+  glColor3f(1.0f, 1.0f, 1.0f);
+  glRasterPos2i(_x, _y);
+  void *font = GLUT_BITMAP_9_BY_15;
+  for (auto c : _text)
+    glutBitmapCharacter(font, c);
+  glMatrixMode(GL_MODELVIEW);
+  glPopMatrix();
+  glMatrixMode(GL_PROJECTION);
+  glPopMatrix();
+}
+
 
 //////////////////////////////////////////////////
 void displayCB()
@@ -241,6 +297,7 @@ void displayCB()
   }
 #endif
 
+  updateLidarVisual();
   g_cameras[g_cameraIndex]->Capture(*g_image);
   handleMouse();
 
@@ -259,6 +316,11 @@ void displayCB()
   glRasterPos2f(-1, 1);
   glDrawPixels(imgw, imgh, GL_RGB, GL_UNSIGNED_BYTE, data);
 
+  std::stringstream text;
+  text << std::setw(30) << "Update time (microseconds): " << prevUpdateTime;
+  drawText(10, 10, text.str());
+
+
   glutSwapBuffers();
 }
 
@@ -269,6 +331,39 @@ void keyboardCB(unsigned char _key, int, int)
   {
     exit(0);
   }
+  else if (_key == 'h' || _key == 'H')
+  {
+    g_showNonHitting = !g_showNonHitting;
+    g_lidarVisualUpdateDirty = true;
+  }
+  else if (_key == '0')
+  {
+    g_lidarVisType = LidarVisualType::LVT_NONE;
+    g_lidarVisualUpdateDirty = true;
+    ignmsg << "Set lidar visual type to NONE"
+           << std::endl;
+  }
+  else if (_key == '1')
+  {
+    g_lidarVisType = LidarVisualType::LVT_RAY_LINES;
+    g_lidarVisualUpdateDirty = true;
+    ignmsg << "Set lidar visual type to RAY_LINES"
+           << std::endl;
+  }
+  else if (_key == '2')
+  {
+    g_lidarVisType = LidarVisualType::LVT_POINTS;
+    g_lidarVisualUpdateDirty = true;
+    ignmsg << "Set lidar visual type to POINTS"
+           << std::endl;
+  }
+  else if (_key == '3')
+  {
+    g_lidarVisType = LidarVisualType::LVT_TRIANGLE_STRIPS;
+    g_lidarVisualUpdateDirty = true;
+    ignmsg << "Set lidar visual type to TRIANGLE_STRIPS"
+           << std::endl;
+  }
   else if (_key == KEY_TAB)
   {
     g_cameraIndex = (g_cameraIndex + 1) % g_cameras.size();
@@ -293,6 +388,14 @@ void initCamera(ir::CameraPtr _camera)
   g_camera->Capture(*g_image);
 }
 
+
+//////////////////////////////////////////////////
+void initLidarVisual(ir::LidarVisualPtr _lidar)
+{
+  g_lidar = _lidar;
+}
+
+
 //////////////////////////////////////////////////
 void initContext()
 {
@@ -303,7 +406,6 @@ void initContext()
   glutDisplayFunc(displayCB);
   glutIdleFunc(idleCB);
   glutKeyboardFunc(keyboardCB);
-
   glutMouseFunc(mouseCB);
   glutMotionFunc(motionCB);
 }
@@ -315,11 +417,20 @@ void printUsage()
   std::cout << "==========================================" << std::endl;
   std::cout << "  TAB - Switch render engines             " << std::endl;
   std::cout << "  ESC - Exit                              " << std::endl;
+  std::cout << "                                          " << std::endl;
+  std::cout << "  H: Toggle display for non-hitting rays  " << std::endl;
+  std::cout << "                                          " << std::endl;
+  std::cout << "  0: Do not display visual                " << std::endl;
+  std::cout << "  1: Display ray lines visual             " << std::endl;
+  std::cout << "  2: Display points visual                " << std::endl;
+  std::cout << "  3: Display triangle strips visual       " << std::endl;
   std::cout << "==========================================" << std::endl;
 }
 
 //////////////////////////////////////////////////
-void run(std::vector<ir::CameraPtr> _cameras)
+void run(std::vector<ir::CameraPtr> _cameras,
+        std::vector<ir::LidarVisualPtr> _nodes,
+        std::vector<double> _pts)
 {
   if (_cameras.empty())
   {
@@ -339,8 +450,14 @@ void run(std::vector<ir::CameraPtr> _cameras)
   g_cameras = _cameras;
 
   initCamera(_cameras[0]);
+  initLidarVisual(_nodes[0]);
   initContext();
   printUsage();
+  g_lidarData.clear();
+  for (int pt =0; pt < _pts.size(); pt++)
+  {
+    g_lidarData.push_back(_pts[pt]);
+  }
 
 #if __APPLE__
   g_glutContext = CGLGetCurrentContext();

examples/lidar_visual/GlutWindow.hh

@@ -26,6 +26,8 @@ namespace ic = ignition::common;
 
 /// \brief Run the demo and display Lidar Visual
 /// \param[in] _cameras Cameras in the scene
-void run(std::vector<ir::CameraPtr> _cameras);
+void run(std::vector<ir::CameraPtr> _cameras,
+         std::vector<ir::LidarVisualPtr> _nodes,
+         std::vector<double> _pts);
 
 #endif

examples/lidar_visual/Main.cc

@@ -36,9 +36,23 @@
 using namespace ignition;
 using namespace rendering;
 
+
 const std::string RESOURCE_PATH =
     common::joinPaths(std::string(PROJECT_BINARY_PATH), "media");
 
+// paramters for the LidarVisual and GpuRays API are initialised here
+const double hMinAngle = -2.26889;
+const double hMaxAngle = 2.26889;
+const double vMinAngle = 0;
+const double vMaxAngle = 0.1;
+const double minRange = 0.08;
+const double maxRange = 10.0;
+const int hRayCount = 640;
+const int vRayCount = 1;
+std::vector<double> pts;
+
+ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.5),
+    ignition::math::Quaterniond::Identity);
 
 //////////////////////////////////////////////////
 void OnNewGpuRaysFrame(float *_scanDest, const float *_scan,
@@ -131,21 +145,25 @@ void buildScene(ScenePtr _scene)
   visualSphere1->SetMaterial(yellow);
   root->AddChild(visualSphere1);
 
+  // create camera
+  CameraPtr camera = _scene->CreateCamera("camera");
+  camera->SetLocalPosition(0.0, 0.0, 2.0);
+  camera->SetLocalRotation(0.0, 0.5, 0.0);
+  camera->SetImageWidth(1200);
+  camera->SetImageHeight(900);
+  camera->SetAntiAliasing(2);
+  camera->SetAspectRatio(1.333);
+  camera->SetHFOV(IGN_PI / 2);
+  root->AddChild(camera);
+}
+
+//////////////////////////////////////////////////
+GpuRaysPtr createGpuRaySensor(ScenePtr _scene)
+{
   // set parameters for GPU lidar sensor and visualisations
   // parameters are based on a sample 2D planar laser sensor
-  ignition::math::Pose3d testPose(ignition::math::Vector3d(0, 0, 0.5),
-      ignition::math::Quaterniond::Identity);
-  const double hMinAngle = -2.26889;
-  const double hMaxAngle = 2.26889;
-  const double vMinAngle = 0;
-  const double vMaxAngle = 0.1;
-  const double minRange = 0.08;
-  const double maxRange = 10.0;
-  const int hRayCount = 640;
-  const int vRayCount = 1;
-
   // add GPU lidar sensor and set parameters
-  GpuRaysPtr gpuRays = _scene->CreateGpuRays("gpu_rays_1");
+  GpuRaysPtr gpuRays = _scene->CreateGpuRays("gpu_rays");
   gpuRays->SetWorldPosition(testPose.Pos());
   gpuRays->SetWorldRotation(testPose.Rot());
   gpuRays->SetNearClipPlane(minRange);
@@ -156,6 +174,8 @@ void buildScene(ScenePtr _scene)
   gpuRays->SetVerticalAngleMin(vMinAngle);
   gpuRays->SetVerticalAngleMax(vMaxAngle);
   gpuRays->SetVerticalRayCount(vRayCount);
+
+  VisualPtr root = _scene->RootVisual();
   root->AddChild(gpuRays);
 
   unsigned int channels = gpuRays->Channels();
@@ -170,16 +190,20 @@ void buildScene(ScenePtr _scene)
   // update the sensor data
   gpuRays->Update();
 
-  // extract range values from GPU lidar data
-  std::vector<double> pts;
+  pts.clear();
   for (int j = 0; j < vRayCount; j++)
   {
     for (int i = 0; i < gpuRays->RayCount(); ++i)
     {
       pts.push_back(scan[j*channels*gpuRays->RayCount() + i * channels]);
     }
   }
+  return gpuRays;
+}
 
+//////////////////////////////////////////////////
+LidarVisualPtr createLidar(ScenePtr _scene)
+{
   // create lidar visual
   LidarVisualPtr lidar = _scene->CreateLidarVisual();
   lidar->SetMinHorizontalAngle(hMinAngle);
@@ -190,24 +214,29 @@ void buildScene(ScenePtr _scene)
   lidar->SetMaxVerticalAngle(vMaxAngle);
   lidar->SetMaxRange(maxRange);
   lidar->SetMinRange(minRange);
+
+  // the types can be set as follows:-
+  // LVT_POINTS -> Lidar Points at the range value
+  // LVT_RAY_LINES -> Lines along the lidar sensor to the obstacle
+  // LVT_TRIANGLE_STRIPS -> Coloured triangle strips denoting hitting and
+  // non-hitting parts of the scan
+  lidar->SetType(LidarVisualType::LVT_RAY_LINES);
   lidar->SetPoints(pts);
+
+  VisualPtr root = _scene->RootVisual();
+  root->AddChild(lidar);
+
+  // set this value to false if only the rays that are hitting another obstacle
+  // are to be displayed.
+  lidar->SetDisplayNonHitting(true);
+
   lidar->SetWorldPosition(testPose.Pos());
   lidar->SetWorldRotation(testPose.Rot());
-  root->AddChild(lidar);
 
   // update lidar visual
   lidar->Update();
 
-  // create camera
-  CameraPtr camera = _scene->CreateCamera("camera");
-  camera->SetLocalPosition(0.0, 0.0, 2.0);
-  camera->SetLocalRotation(0.0, 0.5, 0.0);
-  camera->SetImageWidth(1200);
-  camera->SetImageHeight(900);
-  camera->SetAntiAliasing(2);
-  camera->SetAspectRatio(1.333);
-  camera->SetHFOV(IGN_PI / 2);
-  root->AddChild(camera);
+  return lidar;
 }
 
 //////////////////////////////////////////////////
@@ -245,6 +274,8 @@ int main(int _argc, char** _argv)
   common::Console::SetVerbosity(4);
   std::vector<std::string> engineNames;
   std::vector<CameraPtr> cameras;
+  std::vector<LidarVisualPtr> nodes;
+  std::vector<GpuRaysPtr> sensors;
 
   engineNames.push_back(engine);
 
@@ -257,13 +288,16 @@ int main(int _argc, char** _argv)
       if (camera)
       {
         cameras.push_back(camera);
+        sensors.push_back(createGpuRaySensor(camera->Scene()));
+        nodes.push_back(createLidar(camera->Scene()));
       }
     }
     catch (...)
     {
       std::cerr << "Error starting up: " << engineName << std::endl;
     }
   }
-  run(cameras);
+
+  run(cameras, nodes, nodes[0]->Points());
   return 0;
 }