Skip to content

Commit

Permalink
Add types for lidar visual (#114)
Browse files Browse the repository at this point in the history 
* added types for lidar visual, option to display non-hitting rays

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* example updated

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* updated lidar_visual test

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Linter Error fix

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Edited code to make it shorter

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Fixed update slowdown error

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Fixed unrelated linter errors

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Working keypress example

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* destroy function update

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* clear visual data for update function

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Fixed Destroy errors

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Minor fixes

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Can set id and name of lidar visual

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* Remove space

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* removed whitespaces

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* fixed document arguments

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

* fix windows CI build error

Signed-off-by: Mihir Kulkarni <mihirk284@gmail.com>

Co-authored-by: Alejandro Hernández Cordero <ahcorde@gmail.com>
  • Loading branch information
@mihirk284 @ahcorde
mihirk284 and ahcorde committed Jul 22, 2020
1 parent f2e838b commit e22fd80
Show file tree
Hide file tree
Showing 15 changed files with 591 additions and 133 deletions.
3 changes: 3 additions & 0 deletions Changelog.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down
118 changes: 114 additions & 4 deletions examples/lidar_visual/GlutWindow.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand All @@ -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;
Expand Down Expand Up @@ -227,6 +241,46 @@ 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()
Expand All @@ -241,6 +295,7 @@ void displayCB()
}
#endif

updateLidarVisual();
g_cameras[g_cameraIndex]->Capture(*g_image);
handleMouse();

Expand All @@ -259,6 +314,10 @@ 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();
}

Expand All @@ -269,6 +328,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();
Expand All @@ -281,7 +373,6 @@ void idleCB()
glutPostRedisplay();
}


//////////////////////////////////////////////////
void initCamera(ir::CameraPtr _camera)
{
Expand All @@ -293,6 +384,12 @@ void initCamera(ir::CameraPtr _camera)
g_camera->Capture(*g_image);
}

//////////////////////////////////////////////////
void initLidarVisual(ir::LidarVisualPtr _lidar)
{
g_lidar = _lidar;
}

//////////////////////////////////////////////////
void initContext()
{
Expand All @@ -303,23 +400,30 @@ void initContext()
glutDisplayFunc(displayCB);
glutIdleFunc(idleCB);
glutKeyboardFunc(keyboardCB);

glutMouseFunc(mouseCB);
glutMotionFunc(motionCB);
}


//////////////////////////////////////////////////
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())
{
Expand All @@ -339,8 +443,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();
Expand Down
6 changes: 5 additions & 1 deletion examples/lidar_visual/GlutWindow.hh
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,10 @@ 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);
/// \param[in] nodes Nodes in the scene
/// \param[in] _pts Lidar points
void run(std::vector<ir::CameraPtr> _cameras,
std::vector<ir::LidarVisualPtr> _nodes,
std::vector<double> _pts);

#endif
85 changes: 59 additions & 26 deletions examples/lidar_visual/Main.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,19 @@ 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,
Expand Down Expand Up @@ -131,21 +144,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);
Expand All @@ -156,6 +173,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();
Expand All @@ -170,16 +189,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);
Expand All @@ -190,24 +213,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;
}

//////////////////////////////////////////////////
Expand Down Expand Up @@ -245,6 +273,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);

Expand All @@ -257,13 +287,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;
}

0 comments on commit e22fd80

Please sign in to comment.