From 7e0f31dccc8a2fb1ffa9840e2b551f907389c502 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 13 Jul 2020 18:16:13 +0530 Subject: [PATCH 01/17] added types for lidar visual, option to display non-hitting rays Signed-off-by: Mihir Kulkarni --- include/ignition/rendering/LidarVisual.hh | 37 +++- .../rendering/base/BaseLidarVisual.hh | 49 ++++- ogre/src/OgreLidarVisual.cc | 199 +++++++++++++++--- 3 files changed, 248 insertions(+), 37 deletions(-) diff --git a/include/ignition/rendering/LidarVisual.hh b/include/ignition/rendering/LidarVisual.hh index 06ab72ed2..35b77c5d1 100644 --- a/include/ignition/rendering/LidarVisual.hh +++ b/include/ignition/rendering/LidarVisual.hh @@ -32,6 +32,23 @@ namespace ignition namespace rendering { inline namespace IGNITION_RENDERING_VERSION_NAMESPACE { + // + /// \brief Enum for LidarVisual types + enum IGNITION_RENDERING_VISIBLE LidarVisualType + { + /// \brief No type + LVT_NONE = 0, + + /// \brief Ray line visual + LVT_RAY_LINES = 1, + + /// \brief Points visual + LVT_POINTS = 2, + + /// \brief Triangle strips visual + LVT_TRIANGLE_STRIPS = 3 + }; + /// \class LidarVisual LidarVisual.hh ignition/rendering/LidarVisual /// \brief A LidarVisual geometry class. The visual appearance is based /// on the type specified. @@ -139,9 +156,25 @@ namespace ignition /// \brief Get the points in laser data /// \return The points in the laser data public: virtual std::vector Points() const = 0; + + /// \brief Set type for lidar visual + /// \param[in] _type The type of visualisation for lidar data + public: virtual void SetType(const LidarVisualType _type) = 0; + + /// \brief Get the type for lidar visual + /// \return The type for lidar visual + public: virtual LidarVisualType Type() const = 0; + + /// \brief Set if non-hitting rays will be displayed + /// (this does not work for TRIANGLE_STRIPS visual) + /// \param[in] _display Boolean value to display non hitting visuals + public: virtual void SetDisplayNonHitting(bool _display) = 0; + + /// \brief Get if non-hitting rays will be displayed + /// \return Boolean value if non-hitting rays will be displayed + public: virtual bool DisplayNonHitting() const = 0; }; } } } - -#endif +#endif \ No newline at end of file diff --git a/include/ignition/rendering/base/BaseLidarVisual.hh b/include/ignition/rendering/base/BaseLidarVisual.hh index d4be2653b..b09503a1d 100644 --- a/include/ignition/rendering/base/BaseLidarVisual.hh +++ b/include/ignition/rendering/base/BaseLidarVisual.hh @@ -127,9 +127,21 @@ namespace ignition // Documentation inherited public: virtual std::vector Points() const override; + // Documentation inherited + public: virtual void SetType(const LidarVisualType _type) override; + + // Documentation inherited + public: virtual LidarVisualType Type() const override; + /// \brief Create predefined materials for lidar visual public: virtual void CreateMaterials(); + // Documentation inherited + public: virtual void SetDisplayNonHitting(bool _display) override; + + // Documentation inherited + public: virtual bool DisplayNonHitting() const override; + /// \brief Vertical minimal angle protected: double minVerticalAngle = 0; @@ -160,8 +172,15 @@ namespace ignition /// \brief Maximum Range protected: double maxRange = 0; + /// \brief Minimum Range + protected: bool displayNonHitting = true; + /// \brief Offset of visual protected: ignition::math::Pose3d offset = ignition::math::Pose3d::Zero; + + /// \brief Type of lidar visualisation + protected: LidarVisualType lidarVisualType = + LidarVisualType::LVT_TRIANGLE_STRIPS; }; ///////////////////////////////////////////////// @@ -386,6 +405,34 @@ namespace ignition return this->offset; } + ///////////////////////////////////////////////// + template + void BaseLidarVisual::SetType(const LidarVisualType _type) + { + this->lidarVisualType = _type; + } + + ///////////////////////////////////////////////// + template + LidarVisualType BaseLidarVisual::Type() const + { + return this->lidarVisualType; + } + + ///////////////////////////////////////////////// + template + void BaseLidarVisual::SetDisplayNonHitting(bool _display) + { + this->displayNonHitting = _display; + } + + ///////////////////////////////////////////////// + template + bool BaseLidarVisual::DisplayNonHitting() const + { + return this->displayNonHitting; + } + ///////////////////////////////////////////////// template void BaseLidarVisual::CreateMaterials() @@ -444,4 +491,4 @@ namespace ignition } } } -#endif +#endif \ No newline at end of file diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 2bf17c4e8..dd45c194f 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -37,6 +37,13 @@ class ignition::rendering::OgreLidarVisualPrivate /// \brief Lidar Ray DynamicLines Object to display public: std::vector> rayLines; + /// \brief Lidar Points DynamicLines Object to display + public: std::vector> points; + + /// \brief Lidar visual type + public: LidarVisualType lidarVisType = + LidarVisualType::LVT_TRIANGLE_STRIPS; + /// \brief The current lidar points data public: std::vector lidarPoints; @@ -92,6 +99,12 @@ void OgreLidarVisual::Destroy() ray.reset(); } + for (auto ray : this->dataPtr->points) + { + ray.get()->Clear(); + ray.reset(); + } + this->ClearPoints(); } @@ -123,6 +136,7 @@ void OgreLidarVisual::ClearVisualData() this->dataPtr->deadZoneRayFans.clear(); this->dataPtr->rayLines.clear(); this->dataPtr->rayStrips.clear(); + this->dataPtr->points.clear(); } ////////////////////////////////////////////////// @@ -142,6 +156,13 @@ void OgreLidarVisual::Update() return; } + // if visual type is changed, clear all DynamicLines + if (this->lidarVisualType != this->dataPtr->lidarVisType) + { + this->ClearVisualData(); + } + this->dataPtr->lidarVisType = this->lidarVisualType; + this->dataPtr->receivedData = false; double horizontalAngle = this->minHorizontalAngle; double verticalAngle = this->minVerticalAngle; @@ -171,13 +192,38 @@ void OgreLidarVisual::Update() // Process each point from received data // Every line segment, and every triangle is saved separately, // as a pointer to a DynamicLine + // This initializes and updates only the selected DynamicLine variables for (unsigned int j = 0; j < this->verticalCount; ++j) { horizontalAngle = this->minHorizontalAngle; - if (j+1 > this->dataPtr->rayLines.size()) + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + { + if (j+1 > this->dataPtr->rayLines.size()) + { + std::shared_ptr line = + std::shared_ptr( + new OgreDynamicLines(MT_LINE_LIST)); + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + line->setMaterial("Lidar/BlueRay"); + #else + line->setMaterial( + this->Scene()->Material("Lidar/BlueRay")->Clone()); + #endif + std::shared_ptr mv = + std::dynamic_pointer_cast(line); + this->Node()->attachObject(mv.get()); + this->dataPtr->rayLines.push_back(line); + } + } + + else if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) { - // Ray Strips fill in between the line areas that intersect an object + if (j+1 > this->dataPtr->rayLines.size()) + { + // Ray Strips fill in between the line areas that intersect an object std::shared_ptr line = std::shared_ptr( new OgreDynamicLines(MT_TRIANGLE_STRIP)); @@ -225,7 +271,6 @@ void OgreLidarVisual::Update() this->dataPtr->deadZoneRayFans[j]->AddPoint( ignition::math::Vector3d::Zero); - line = std::shared_ptr( new OgreDynamicLines(MT_LINE_LIST)); @@ -239,9 +284,31 @@ void OgreLidarVisual::Update() mv = std::dynamic_pointer_cast(line); this->Node()->attachObject(mv.get()); this->dataPtr->rayLines.push_back(line); + } + this->dataPtr->deadZoneRayFans[j]->SetPoint(0, this->offset.Pos()); } - this->dataPtr->deadZoneRayFans[j]->SetPoint(0, this->offset.Pos()); + else if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_POINTS) + { + if (j+1 > this->dataPtr->points.size()) + { + std::shared_ptr line = + std::shared_ptr( + new OgreDynamicLines(MT_POINTS)); + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + line->setMaterial("Default/White"); + #else + line->setMaterial( + this->Scene()->Material("Default/White")->Clone()); + #endif + std::shared_ptr mv = + std::dynamic_pointer_cast(line); + this->Node()->attachObject(mv.get()); + this->dataPtr->points.push_back(line); + } + } unsigned count = this->horizontalCount; // Process each ray in current scan @@ -275,41 +342,105 @@ void OgreLidarVisual::Update() ignition::math::Vector3d noHitPt = (axis * noHitRange) + this->offset.Pos(); - // Draw the lines and strips that represent each simulated ray - if (i >= this->dataPtr->rayLines[j]->PointCount()/2) - { - this->dataPtr->rayLines[j]->AddPoint(startPt); - this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); - - this->dataPtr->rayStrips[j]->AddPoint(startPt); - this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); + // Update the lines and strips that represent each simulated ray. - this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); - this->dataPtr->noHitRayStrips[j]->AddPoint(inf ? noHitPt : pt); - } - else + // For RAY_LINES Lidar Visual to be displayed + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_RAY_LINES) { - this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); - this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); - - this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf ? startPt : pt); + if (i >= this->dataPtr->rayLines[j]->PointCount()/2) + { + if (this->displayNonHitting || !inf) + { + this->dataPtr->rayLines[j]->AddPoint(startPt); + this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); + } + } + else + { + if (this->displayNonHitting || !inf) + { + this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); + this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); + } + } + } - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, inf ? noHitPt : pt); + // For TRIANGLE_STRIPS Lidar Visual to be displayed + else if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + if (i >= this->dataPtr->rayLines[j]->PointCount()/2) + { + { + this->dataPtr->rayLines[j]->AddPoint(startPt); + this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); + + this->dataPtr->rayStrips[j]->AddPoint(startPt); + this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); + this->dataPtr->noHitRayStrips[j]->AddPoint(inf ? noHitPt : pt); + } + } + else + { + { + this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); + this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); + + this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf ? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, + inf ? noHitPt : pt); + } + } + // Draw the triangle fan that indicates the dead zone. + if (i+1 >= this->dataPtr->deadZoneRayFans[j]->PointCount()) + this->dataPtr->deadZoneRayFans[j]->AddPoint(startPt); + else + this->dataPtr->deadZoneRayFans[j]->SetPoint(i+1, startPt); } - // Draw the triangle fan that indicates the dead zone. - if (i+1 >= this->dataPtr->deadZoneRayFans[j]->PointCount()) - this->dataPtr->deadZoneRayFans[j]->AddPoint(startPt); - else - this->dataPtr->deadZoneRayFans[j]->SetPoint(i+1, startPt); + // For POINTS Lidar Visual to be displayed + else if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_POINTS) + { + if (i >= this->dataPtr->points[j]->PointCount()) + { + if (this->displayNonHitting || !inf) + { + this->dataPtr->points[j]->AddPoint(inf ? noHitPt : pt); + } + } + else + { + if (this->displayNonHitting || !inf) + { + this->dataPtr->points[j]->SetPoint(i, inf ? noHitPt : pt); + } + } + } - // Update all the DynamicLines after adding points - this->dataPtr->rayLines[j]->Update(); - this->dataPtr->rayStrips[j]->Update(); - this->dataPtr->noHitRayStrips[j]->Update(); - this->dataPtr->deadZoneRayFans[j]->Update(); + // Update the DynamicLines pointers after adding points based on type + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + { + this->dataPtr->rayLines[j]->Update(); + } + else if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayLines[j]->Update(); + this->dataPtr->rayStrips[j]->Update(); + this->dataPtr->noHitRayStrips[j]->Update(); + this->dataPtr->deadZoneRayFans[j]->Update(); + } + else if (this->dataPtr->lidarVisType == LidarVisualType::LVT_POINTS) + { + this->dataPtr->points[j]->Update(); + } horizontalAngle += this->horizontalAngleStep; } @@ -327,4 +458,4 @@ unsigned int OgreLidarVisual::PointCount() const std::vector OgreLidarVisual::Points() const { return this->dataPtr->lidarPoints; -} +} \ No newline at end of file From ca80ab1377062d2dd57bf1944c63315b48be0fb1 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 13 Jul 2020 18:16:23 +0530 Subject: [PATCH 02/17] example updated Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/Main.cc | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/examples/lidar_visual/Main.cc b/examples/lidar_visual/Main.cc index fa9229bb3..e939af3fe 100644 --- a/examples/lidar_visual/Main.cc +++ b/examples/lidar_visual/Main.cc @@ -190,7 +190,20 @@ 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); + + // set this value to false if only the rays that are hitting another obstacle + // are to be displayed. + // This does NOT work for LVT_TRIANGLE_STRIPS + lidar->SetDisplayNonHitting(true); + lidar->SetWorldPosition(testPose.Pos()); lidar->SetWorldRotation(testPose.Rot()); root->AddChild(lidar); @@ -266,4 +279,4 @@ int main(int _argc, char** _argv) } run(cameras); return 0; -} +} \ No newline at end of file From 4eadd0da19d9c0b3b89778c41ac92a2714a97967 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 13 Jul 2020 18:16:38 +0530 Subject: [PATCH 03/17] updated lidar_visual test Signed-off-by: Mihir Kulkarni --- src/LidarVisual_TEST.cc | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/src/LidarVisual_TEST.cc b/src/LidarVisual_TEST.cc index a6e3780d4..3ac8d03eb 100644 --- a/src/LidarVisual_TEST.cc +++ b/src/LidarVisual_TEST.cc @@ -93,6 +93,21 @@ void LidarVisualTest::LidarVisual(const std::string &_renderEngine) lidar->SetMinRange(0.54); EXPECT_DOUBLE_EQ(lidar->MinRange(), 0.54); + lidar->SetDisplayNonHitting(false); + EXPECT_EQ(lidar->DisplayNonHitting(), false); + lidar->SetDisplayNonHitting(true); + EXPECT_EQ(lidar->DisplayNonHitting(), true); + + lidar->SetType(LVT_NONE); + EXPECT_EQ(lidar->Type(), LVT_NONE); + lidar->SetType(LVT_POINTS); + EXPECT_EQ(lidar->Type(), LVT_POINTS); + lidar->SetType(LVT_POINTS); + EXPECT_EQ(lidar->Type(), LVT_POINTS); + lidar->SetType(LVT_TRIANGLE_STRIPS); + EXPECT_EQ(lidar->Type(), LVT_TRIANGLE_STRIPS); + + ignition::math::Pose3d p(0.5, 2.56, 3.67, 1.4, 2, 4.5); lidar->SetOffset(p); EXPECT_EQ(lidar->Offset(), p); @@ -125,4 +140,4 @@ int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} +} \ No newline at end of file From b0dd066adccae47eb804f43947429d0733007056 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 13 Jul 2020 18:29:52 +0530 Subject: [PATCH 04/17] Linter Error fix Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/Main.cc | 2 +- include/ignition/rendering/LidarVisual.hh | 2 +- include/ignition/rendering/base/BaseLidarVisual.hh | 2 +- ogre/src/OgreLidarVisual.cc | 2 +- src/LidarVisual_TEST.cc | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/lidar_visual/Main.cc b/examples/lidar_visual/Main.cc index e939af3fe..ef7e3c8a8 100644 --- a/examples/lidar_visual/Main.cc +++ b/examples/lidar_visual/Main.cc @@ -202,7 +202,7 @@ void buildScene(ScenePtr _scene) // set this value to false if only the rays that are hitting another obstacle // are to be displayed. // This does NOT work for LVT_TRIANGLE_STRIPS - lidar->SetDisplayNonHitting(true); + lidar->SetDisplayNonHitting(false); lidar->SetWorldPosition(testPose.Pos()); lidar->SetWorldRotation(testPose.Rot()); diff --git a/include/ignition/rendering/LidarVisual.hh b/include/ignition/rendering/LidarVisual.hh index 35b77c5d1..812410940 100644 --- a/include/ignition/rendering/LidarVisual.hh +++ b/include/ignition/rendering/LidarVisual.hh @@ -177,4 +177,4 @@ namespace ignition } } } -#endif \ No newline at end of file +#endif diff --git a/include/ignition/rendering/base/BaseLidarVisual.hh b/include/ignition/rendering/base/BaseLidarVisual.hh index b09503a1d..8b240c974 100644 --- a/include/ignition/rendering/base/BaseLidarVisual.hh +++ b/include/ignition/rendering/base/BaseLidarVisual.hh @@ -491,4 +491,4 @@ namespace ignition } } } -#endif \ No newline at end of file +#endif diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index dd45c194f..397e3a9cf 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -458,4 +458,4 @@ unsigned int OgreLidarVisual::PointCount() const std::vector OgreLidarVisual::Points() const { return this->dataPtr->lidarPoints; -} \ No newline at end of file +} diff --git a/src/LidarVisual_TEST.cc b/src/LidarVisual_TEST.cc index 3ac8d03eb..a205b76a8 100644 --- a/src/LidarVisual_TEST.cc +++ b/src/LidarVisual_TEST.cc @@ -140,4 +140,4 @@ int main(int argc, char **argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); -} \ No newline at end of file +} From 78d12f1da8b620c2853caaee93d93cb5c562b3bb Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 14 Jul 2020 17:36:56 +0530 Subject: [PATCH 05/17] Edited code to make it shorter Signed-off-by: Mihir Kulkarni --- Changelog.md | 3 + .../rendering/base/BaseLidarVisual.hh | 2 +- ogre/src/OgreLidarVisual.cc | 242 ++++++++---------- 3 files changed, 117 insertions(+), 130 deletions(-) diff --git a/Changelog.md b/Changelog.md index d6fcb6f75..fbfeb1a06 100644 --- a/Changelog.md +++ b/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) diff --git a/include/ignition/rendering/base/BaseLidarVisual.hh b/include/ignition/rendering/base/BaseLidarVisual.hh index 8b240c974..4ae482b27 100644 --- a/include/ignition/rendering/base/BaseLidarVisual.hh +++ b/include/ignition/rendering/base/BaseLidarVisual.hh @@ -172,7 +172,7 @@ namespace ignition /// \brief Maximum Range protected: double maxRange = 0; - /// \brief Minimum Range + /// \brief Option to display non-hitting rays protected: bool displayNonHitting = true; /// \brief Offset of visual diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 397e3a9cf..2a0c2e00a 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -197,7 +197,8 @@ void OgreLidarVisual::Update() { horizontalAngle = this->minHorizontalAngle; - if (this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES || + this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) { if (j+1 > this->dataPtr->rayLines.size()) { @@ -208,84 +209,63 @@ void OgreLidarVisual::Update() #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) line->setMaterial("Lidar/BlueRay"); #else - line->setMaterial( - this->Scene()->Material("Lidar/BlueRay")->Clone()); + Ogre::MaterialPtr mat = + Ogre::MaterialManager::getSingleton().getByName( + "Lidar/BlueRay"); + line->setMaterial(mat); #endif std::shared_ptr mv = std::dynamic_pointer_cast(line); this->Node()->attachObject(mv.get()); this->dataPtr->rayLines.push_back(line); - } - } - else if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - if (j+1 > this->dataPtr->rayLines.size()) - { - // Ray Strips fill in between the line areas that intersect an object - std::shared_ptr line = - std::shared_ptr( - new OgreDynamicLines(MT_TRIANGLE_STRIP)); - #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) - line->setMaterial("Lidar/BlueStrips"); - #else - Ogre::MaterialPtr mat = - Ogre::MaterialManager::getSingleton().getByName( - "Lidar/BlueStrips"); - line->setMaterial(mat); - #endif - std::shared_ptr mv = - std::dynamic_pointer_cast(line); - - this->Node()->attachObject(mv.get()); - this->dataPtr->rayStrips.push_back(line); - - line = std::shared_ptr( - new OgreDynamicLines(MT_TRIANGLE_STRIP)); - - #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) - line->setMaterial("Lidar/LightBlueStrips"); - #else - mat = Ogre::MaterialManager::getSingleton().getByName( - "Lidar/LightBlueStrips"); - line->setMaterial(mat); - #endif - mv = std::dynamic_pointer_cast(line); - this->Node()->attachObject(mv.get()); - this->dataPtr->noHitRayStrips.push_back(line); - - line = std::shared_ptr( - new OgreDynamicLines(MT_TRIANGLE_FAN)); - - #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) - line->setMaterial("Lidar/TransBlack"); - #else - mat = Ogre::MaterialManager::getSingleton().getByName( - "Lidar/TransBlack"); - line->setMaterial(mat); - #endif - mv = std::dynamic_pointer_cast(line); - this->Node()->attachObject(mv.get()); - this->dataPtr->deadZoneRayFans.push_back(line); - this->dataPtr->deadZoneRayFans[j]->AddPoint( - ignition::math::Vector3d::Zero); - - line = std::shared_ptr( - new OgreDynamicLines(MT_LINE_LIST)); - - #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) - line->setMaterial("Lidar/BlueRay"); - #else - mat = Ogre::MaterialManager::getSingleton().getByName( - "Lidar/BlueRay"); - line->setMaterial(mat); - #endif - mv = std::dynamic_pointer_cast(line); - this->Node()->attachObject(mv.get()); - this->dataPtr->rayLines.push_back(line); + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) + { + line = std::shared_ptr( + new OgreDynamicLines(MT_TRIANGLE_STRIP)); + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + line->setMaterial("Lidar/LightBlueStrips"); + #else + mat = Ogre::MaterialManager::getSingleton().getByName( + "Lidar/LightBlueStrips"); + line->setMaterial(mat); + #endif + mv = std::dynamic_pointer_cast(line); + this->Node()->attachObject(mv.get()); + this->dataPtr->noHitRayStrips.push_back(line); + + line = std::shared_ptr( + new OgreDynamicLines(MT_TRIANGLE_FAN)); + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + line->setMaterial("Lidar/TransBlack"); + #else + mat = Ogre::MaterialManager::getSingleton().getByName( + "Lidar/TransBlack"); + line->setMaterial(mat); + #endif + mv = std::dynamic_pointer_cast(line); + this->Node()->attachObject(mv.get()); + this->dataPtr->deadZoneRayFans.push_back(line); + this->dataPtr->deadZoneRayFans[j]->AddPoint( + ignition::math::Vector3d::Zero); + + line = std::shared_ptr( + new OgreDynamicLines(MT_TRIANGLE_STRIP)); + + #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) + line->setMaterial("Lidar/BlueStrips"); + #else + mat = Ogre::MaterialManager::getSingleton().getByName( + "Lidar/BlueStrips"); + line->setMaterial(mat); + #endif + mv = std::dynamic_pointer_cast(line); + this->Node()->attachObject(mv.get()); + this->dataPtr->rayStrips.push_back(line); + } } - this->dataPtr->deadZoneRayFans[j]->SetPoint(0, this->offset.Pos()); } else if (this->dataPtr->lidarVisType == @@ -298,10 +278,11 @@ void OgreLidarVisual::Update() new OgreDynamicLines(MT_POINTS)); #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) - line->setMaterial("Default/White"); + line->setMaterial("Lidar/BlueRay"); #else - line->setMaterial( - this->Scene()->Material("Default/White")->Clone()); + mat = Ogre::MaterialManager::getSingleton().getByName( + "Lidar/BlueRay"); + line->setMaterial(mat); #endif std::shared_ptr mv = std::dynamic_pointer_cast(line); @@ -310,6 +291,11 @@ void OgreLidarVisual::Update() } } + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->deadZoneRayFans[j]->SetPoint(0, this->offset.Pos()); + } + unsigned count = this->horizontalCount; // Process each ray in current scan for (unsigned int i = 0; i < count; ++i) @@ -344,64 +330,62 @@ void OgreLidarVisual::Update() // Update the lines and strips that represent each simulated ray. - // For RAY_LINES Lidar Visual to be displayed - if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_RAY_LINES) + // For TRIANGLE_STRIPS Lidar Visual to be displayed + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS || + this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) { if (i >= this->dataPtr->rayLines[j]->PointCount()/2) { - if (this->displayNonHitting || !inf) { - this->dataPtr->rayLines[j]->AddPoint(startPt); - this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); + if (this->displayNonHitting || !inf) + { + this->dataPtr->rayLines[j]->AddPoint(startPt); + this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); + } + + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayStrips[j]->AddPoint(startPt); + this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); + this->dataPtr->noHitRayStrips[j]->AddPoint( + this->displayNonHitting ? (inf ? noHitPt : pt) : + (inf ? startPt : pt)); + } } } else { - if (this->displayNonHitting || !inf) { - this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); - this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); + if (this->displayNonHitting || !inf) + { + this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); + this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); + } + + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, + this->displayNonHitting ? (inf ? noHitPt : pt) : + (inf ? startPt : pt)); + } } } - } - - // For TRIANGLE_STRIPS Lidar Visual to be displayed - else if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - if (i >= this->dataPtr->rayLines[j]->PointCount()/2) - { - { - this->dataPtr->rayLines[j]->AddPoint(startPt); - this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); - - this->dataPtr->rayStrips[j]->AddPoint(startPt); - this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); - - this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); - this->dataPtr->noHitRayStrips[j]->AddPoint(inf ? noHitPt : pt); - } - } - else + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) { - { - this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); - this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); - - this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf ? startPt : pt); - - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, - inf ? noHitPt : pt); - } + // Draw the triangle fan that indicates the dead zone. + if (i+1 >= this->dataPtr->deadZoneRayFans[j]->PointCount()) + this->dataPtr->deadZoneRayFans[j]->AddPoint(startPt); + else + this->dataPtr->deadZoneRayFans[j]->SetPoint(i+1, startPt); } - // Draw the triangle fan that indicates the dead zone. - if (i+1 >= this->dataPtr->deadZoneRayFans[j]->PointCount()) - this->dataPtr->deadZoneRayFans[j]->AddPoint(startPt); - else - this->dataPtr->deadZoneRayFans[j]->SetPoint(i+1, startPt); } // For POINTS Lidar Visual to be displayed @@ -425,17 +409,17 @@ void OgreLidarVisual::Update() } // Update the DynamicLines pointers after adding points based on type - if (this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS || + this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) { this->dataPtr->rayLines[j]->Update(); - } - else if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - this->dataPtr->rayLines[j]->Update(); - this->dataPtr->rayStrips[j]->Update(); - this->dataPtr->noHitRayStrips[j]->Update(); - this->dataPtr->deadZoneRayFans[j]->Update(); + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayStrips[j]->Update(); + this->dataPtr->noHitRayStrips[j]->Update(); + this->dataPtr->deadZoneRayFans[j]->Update(); + } } else if (this->dataPtr->lidarVisType == LidarVisualType::LVT_POINTS) { From 8a34ff4133e9bba472755f10024df230155b59af Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 14 Jul 2020 18:48:44 +0530 Subject: [PATCH 06/17] Fixed update slowdown error Signed-off-by: Mihir Kulkarni --- ogre/src/OgreLidarVisual.cc | 40 ++++++++++++++++++------------------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 2a0c2e00a..fa64834fc 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -101,7 +101,7 @@ void OgreLidarVisual::Destroy() for (auto ray : this->dataPtr->points) { - ray.get()->Clear(); + ray->Clear(); ray.reset(); } @@ -331,8 +331,8 @@ void OgreLidarVisual::Update() // Update the lines and strips that represent each simulated ray. // For TRIANGLE_STRIPS Lidar Visual to be displayed - if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS || - this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS + || this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) { if (i >= this->dataPtr->rayLines[j]->PointCount()/2) { @@ -407,27 +407,27 @@ void OgreLidarVisual::Update() } } } + horizontalAngle += this->horizontalAngleStep; + } - // Update the DynamicLines pointers after adding points based on type - if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS || - this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) - { - this->dataPtr->rayLines[j]->Update(); - if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - this->dataPtr->rayStrips[j]->Update(); - this->dataPtr->noHitRayStrips[j]->Update(); - this->dataPtr->deadZoneRayFans[j]->Update(); - } - } - else if (this->dataPtr->lidarVisType == LidarVisualType::LVT_POINTS) + // Update the DynamicLines pointers after adding points based on type + if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS + || this->dataPtr->lidarVisType == LidarVisualType::LVT_RAY_LINES) + { + this->dataPtr->rayLines[j]->Update(); + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) { - this->dataPtr->points[j]->Update(); + this->dataPtr->rayStrips[j]->Update(); + this->dataPtr->noHitRayStrips[j]->Update(); + this->dataPtr->deadZoneRayFans[j]->Update(); } - - horizontalAngle += this->horizontalAngleStep; } + else if (this->dataPtr->lidarVisType == LidarVisualType::LVT_POINTS) + { + this->dataPtr->points[j]->Update(); + } + verticalAngle += this->verticalAngleStep; } } From 6f2d72d2502523787977d418828537ee814d859a Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 14 Jul 2020 20:45:15 +0530 Subject: [PATCH 07/17] Fixed unrelated linter errors Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/Main.cc | 2 +- ogre/src/OgreRenderEngine.cc | 2 +- ogre2/src/Ogre2RenderEngine.cc | 2 +- src/RenderEngineManager.cc | 4 ++-- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/examples/lidar_visual/Main.cc b/examples/lidar_visual/Main.cc index ef7e3c8a8..c630b551d 100644 --- a/examples/lidar_visual/Main.cc +++ b/examples/lidar_visual/Main.cc @@ -279,4 +279,4 @@ int main(int _argc, char** _argv) } run(cameras); return 0; -} \ No newline at end of file +} diff --git a/ogre/src/OgreRenderEngine.cc b/ogre/src/OgreRenderEngine.cc index 92a9dd956..8d0ea7fae 100644 --- a/ogre/src/OgreRenderEngine.cc +++ b/ogre/src/OgreRenderEngine.cc @@ -81,7 +81,7 @@ OgreRenderEngine::OgreRenderEngine() : this->ogrePaths.push_back(OGRE_RESOURCE_PATH); const char *env = std::getenv("OGRE_RESOURCE_PATH"); - if(env) + if (env) this->ogrePaths.push_back(std::string(env)); } diff --git a/ogre2/src/Ogre2RenderEngine.cc b/ogre2/src/Ogre2RenderEngine.cc index bd02431c3..b0543e17b 100644 --- a/ogre2/src/Ogre2RenderEngine.cc +++ b/ogre2/src/Ogre2RenderEngine.cc @@ -83,7 +83,7 @@ Ogre2RenderEngine::Ogre2RenderEngine() : this->ogrePaths.push_back(ogrePath); const char *env = std::getenv("OGRE2_RESOURCE_PATH"); - if(env) + if (env) this->ogrePaths.push_back(std::string(env)); #ifdef __APPLE__ diff --git a/src/RenderEngineManager.cc b/src/RenderEngineManager.cc index b6235071b..67c70adcc 100644 --- a/src/RenderEngineManager.cc +++ b/src/RenderEngineManager.cc @@ -83,7 +83,7 @@ class ignition::rendering::RenderEngineManagerPrivate /// \brief Plugin loader for managing render engine plugin libraries public: ignition::plugin::Loader pluginLoader; - + /// \brief Environment variable which holds paths to look for plugins public: std::string pluginPathEnv = "IGN_RENDERING_PLUGIN_PATH"; @@ -377,7 +377,7 @@ bool RenderEngineManagerPrivate::LoadEnginePlugin( ignition::common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(this->pluginPathEnv); - + // Add default install folder. systemPaths.AddPluginPaths(std::string(IGN_RENDERING_PLUGIN_PATH)); // Add extra search path. From ce5af20c5b93ddc6d3c5cf1ff6c70ea7ad020cab Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 14 Jul 2020 23:15:24 +0530 Subject: [PATCH 08/17] Working keypress example Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/GlutWindow.cc | 121 +++++++++++++++++++++++++++- examples/lidar_visual/GlutWindow.hh | 4 +- examples/lidar_visual/Main.cc | 77 +++++++++++------- ogre/src/OgreLidarVisual.cc | 37 ++++----- 4 files changed, 185 insertions(+), 54 deletions(-) diff --git a/examples/lidar_visual/GlutWindow.cc b/examples/lidar_visual/GlutWindow.cc index e2b5ac5c1..74bcac392 100644 --- a/examples/lidar_visual/GlutWindow.cc +++ b/examples/lidar_visual/GlutWindow.cc @@ -41,6 +41,10 @@ #include #include #include +#include + +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 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( + 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 _cameras) +void run(std::vector _cameras, + std::vector _nodes, + std::vector _pts) { if (_cameras.empty()) { @@ -339,8 +450,14 @@ void run(std::vector _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(); diff --git a/examples/lidar_visual/GlutWindow.hh b/examples/lidar_visual/GlutWindow.hh index 53510482f..c778e154f 100644 --- a/examples/lidar_visual/GlutWindow.hh +++ b/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 _cameras); +void run(std::vector _cameras, + std::vector _nodes, + std::vector _pts); #endif diff --git a/examples/lidar_visual/Main.cc b/examples/lidar_visual/Main.cc index c630b551d..2b0b51922 100644 --- a/examples/lidar_visual/Main.cc +++ b/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 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,8 +190,7 @@ void buildScene(ScenePtr _scene) // update the sensor data gpuRays->Update(); - // extract range values from GPU lidar data - std::vector pts; + pts.clear(); for (int j = 0; j < vRayCount; j++) { for (int i = 0; i < gpuRays->RayCount(); ++i) @@ -179,7 +198,12 @@ void buildScene(ScenePtr _scene) 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); @@ -199,28 +223,20 @@ void buildScene(ScenePtr _scene) 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. - // This does NOT work for LVT_TRIANGLE_STRIPS - lidar->SetDisplayNonHitting(false); + 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; } ////////////////////////////////////////////////// @@ -258,6 +274,8 @@ int main(int _argc, char** _argv) common::Console::SetVerbosity(4); std::vector engineNames; std::vector cameras; + std::vector nodes; + std::vector sensors; engineNames.push_back(engine); @@ -270,6 +288,8 @@ 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 (...) @@ -277,6 +297,7 @@ int main(int _argc, char** _argv) std::cerr << "Error starting up: " << engineName << std::endl; } } - run(cameras); + + run(cameras, nodes, nodes[0]->Points()); return 0; } diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index fa64834fc..63411953d 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -310,6 +310,8 @@ void OgreLidarVisual::Update() ignition::math::Vector3d axis = this->offset.Rot() * ray * ignition::math::Vector3d(1.0, 0.0, 0.0); + ignition::math::Vector3d zeroPt = ignition::math::Vector3d(0.0, 0.0, 0.0); + // Check for infinite range, which indicates the ray did not // intersect an object. double hitRange = inf ? 0 : r; @@ -337,11 +339,9 @@ void OgreLidarVisual::Update() if (i >= this->dataPtr->rayLines[j]->PointCount()/2) { { - if (this->displayNonHitting || !inf) - { - this->dataPtr->rayLines[j]->AddPoint(startPt); - this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt : pt); - } + this->dataPtr->rayLines[j]->AddPoint(startPt); + this->dataPtr->rayLines[j]->AddPoint( + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) @@ -351,19 +351,16 @@ void OgreLidarVisual::Update() this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); this->dataPtr->noHitRayStrips[j]->AddPoint( - this->displayNonHitting ? (inf ? noHitPt : pt) : - (inf ? startPt : pt)); + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); } } } else { { - if (this->displayNonHitting || !inf) - { - this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); - this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt : pt); - } + this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); + this->dataPtr->rayLines[j]->SetPoint(i*2+1, + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) @@ -373,8 +370,7 @@ void OgreLidarVisual::Update() this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, - this->displayNonHitting ? (inf ? noHitPt : pt) : - (inf ? startPt : pt)); + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); } } } @@ -394,17 +390,13 @@ void OgreLidarVisual::Update() { if (i >= this->dataPtr->points[j]->PointCount()) { - if (this->displayNonHitting || !inf) - { - this->dataPtr->points[j]->AddPoint(inf ? noHitPt : pt); - } + this->dataPtr->points[j]->AddPoint( + inf ? (this->displayNonHitting? noHitPt: zeroPt) : pt); } else { - if (this->displayNonHitting || !inf) - { - this->dataPtr->points[j]->SetPoint(i, inf ? noHitPt : pt); - } + this->dataPtr->points[j]->SetPoint(i, + inf ? (this->displayNonHitting? noHitPt: zeroPt) : pt); } } horizontalAngle += this->horizontalAngleStep; @@ -427,7 +419,6 @@ void OgreLidarVisual::Update() { this->dataPtr->points[j]->Update(); } - verticalAngle += this->verticalAngleStep; } } From cb21fa455e7bcd36f453e4651e3d7b87aca42bb9 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Wed, 15 Jul 2020 23:14:30 +0530 Subject: [PATCH 09/17] destroy function update Signed-off-by: Mihir Kulkarni --- ogre/src/OgreLidarVisual.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 63411953d..f3bbccabd 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -75,6 +75,7 @@ void OgreLidarVisual::PreRender() ////////////////////////////////////////////////// void OgreLidarVisual::Destroy() { + BaseLidarVisual::Destroy(); for (auto ray : this->dataPtr->noHitRayStrips) { ray->Clear(); From 1cea60051e29017ccf253cd9e009dddd9791f73e Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Thu, 16 Jul 2020 15:04:45 +0530 Subject: [PATCH 10/17] clear visual data for update function Signed-off-by: Mihir Kulkarni --- ogre/src/OgreLidarVisual.cc | 86 +++++++++++++++++++++++-------------- 1 file changed, 53 insertions(+), 33 deletions(-) diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index f3bbccabd..0f93a47e2 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -44,6 +44,9 @@ class ignition::rendering::OgreLidarVisualPrivate public: LidarVisualType lidarVisType = LidarVisualType::LVT_TRIANGLE_STRIPS; + /// \brief Current value of DisplayNonHitting parameter + public: bool currentDisplayNonHitting = true; + /// \brief The current lidar points data public: std::vector lidarPoints; @@ -157,8 +160,22 @@ void OgreLidarVisual::Update() return; } + bool clearVisuals = false; + + if (this->lidarVisualType != this->dataPtr->lidarVisType + || !this->displayNonHitting) + { + clearVisuals = true; + } + + if (this->displayNonHitting != this->dataPtr->currentDisplayNonHitting) + { + clearVisuals = true; + this->dataPtr->currentDisplayNonHitting = this->displayNonHitting; + } + // if visual type is changed, clear all DynamicLines - if (this->lidarVisualType != this->dataPtr->lidarVisType) + if (clearVisuals) { this->ClearVisualData(); } @@ -174,6 +191,7 @@ void OgreLidarVisual::Update() (this->maxHorizontalAngle - this->minHorizontalAngle) / (this->horizontalCount - 1); } + if (this->verticalCount > 1) { this->verticalAngleStep = @@ -311,8 +329,6 @@ void OgreLidarVisual::Update() ignition::math::Vector3d axis = this->offset.Rot() * ray * ignition::math::Vector3d(1.0, 0.0, 0.0); - ignition::math::Vector3d zeroPt = ignition::math::Vector3d(0.0, 0.0, 0.0); - // Check for infinite range, which indicates the ray did not // intersect an object. double hitRange = inf ? 0 : r; @@ -339,40 +355,40 @@ void OgreLidarVisual::Update() { if (i >= this->dataPtr->rayLines[j]->PointCount()/2) { + if (this->displayNonHitting || !inf) { this->dataPtr->rayLines[j]->AddPoint(startPt); - this->dataPtr->rayLines[j]->AddPoint( - inf ? (this->displayNonHitting? noHitPt: startPt) : pt); - - if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - this->dataPtr->rayStrips[j]->AddPoint(startPt); - this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); - - this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); - this->dataPtr->noHitRayStrips[j]->AddPoint( - inf ? (this->displayNonHitting? noHitPt: startPt) : pt); - } + this->dataPtr->rayLines[j]->AddPoint(inf ? noHitPt: pt); + } + + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayStrips[j]->AddPoint(startPt); + this->dataPtr->rayStrips[j]->AddPoint(inf ? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->AddPoint(startPt); + this->dataPtr->noHitRayStrips[j]->AddPoint( + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); } } else { + if (this->displayNonHitting || !inf) { this->dataPtr->rayLines[j]->SetPoint(i*2, startPt); - this->dataPtr->rayLines[j]->SetPoint(i*2+1, - inf ? (this->displayNonHitting? noHitPt: startPt) : pt); - - if (this->dataPtr->lidarVisType == - LidarVisualType::LVT_TRIANGLE_STRIPS) - { - this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf? startPt : pt); - - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); - this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, - inf ? (this->displayNonHitting? noHitPt: startPt) : pt); - } + this->dataPtr->rayLines[j]->SetPoint(i*2+1, inf ? noHitPt: pt); + } + + if (this->dataPtr->lidarVisType == + LidarVisualType::LVT_TRIANGLE_STRIPS) + { + this->dataPtr->rayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->rayStrips[j]->SetPoint(i*2+1, inf? startPt : pt); + + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2, startPt); + this->dataPtr->noHitRayStrips[j]->SetPoint(i*2+1, + inf ? (this->displayNonHitting? noHitPt: startPt) : pt); } } if (this->dataPtr->lidarVisType == LidarVisualType::LVT_TRIANGLE_STRIPS) @@ -391,13 +407,17 @@ void OgreLidarVisual::Update() { if (i >= this->dataPtr->points[j]->PointCount()) { - this->dataPtr->points[j]->AddPoint( - inf ? (this->displayNonHitting? noHitPt: zeroPt) : pt); + if (this->displayNonHitting || !inf) + { + this->dataPtr->points[j]->AddPoint(inf ? noHitPt: pt); + } } else { - this->dataPtr->points[j]->SetPoint(i, - inf ? (this->displayNonHitting? noHitPt: zeroPt) : pt); + if (this->displayNonHitting || !inf) + { + this->dataPtr->points[j]->SetPoint(i, inf ? noHitPt: pt); + } } } horizontalAngle += this->horizontalAngleStep; From 882539f32a68180e4949dd6380e36acdf7adb27a Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Sun, 19 Jul 2020 21:41:58 +0530 Subject: [PATCH 11/17] Fixed Destroy errors Signed-off-by: Mihir Kulkarni --- ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh | 2 +- ogre/src/OgreLidarVisual.cc | 3 ++- src/base/BaseScene.cc | 6 ++++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh b/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh index de93f234f..ab44addd3 100644 --- a/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh +++ b/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh @@ -51,7 +51,7 @@ namespace ignition public: virtual void PreRender() override; // Documentation inherited. - public: virtual void Destroy() override; + public: virtual void Destroy(); // Documentation inherited public: virtual void Update() override; diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 0f93a47e2..0776c1cfe 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -66,7 +66,7 @@ OgreLidarVisual::OgreLidarVisual() ////////////////////////////////////////////////// OgreLidarVisual::~OgreLidarVisual() { - this->Destroy(); + // no ops } ////////////////////////////////////////////////// @@ -110,6 +110,7 @@ void OgreLidarVisual::Destroy() } this->ClearPoints(); + this->ClearVisualData(); } ////////////////////////////////////////////////// diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index a41692612..f2089c806 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -991,8 +991,10 @@ MarkerPtr BaseScene::CreateMarker() LidarVisualPtr BaseScene::CreateLidarVisual() { unsigned int objId = this->CreateObjectId(); - std::string objName = this->CreateObjectName(objId, "LidarVisual"); - return this->CreateLidarVisualImpl(objId, objName); + const std::string objName = this->CreateObjectName(objId, "LidarVisual"); + LidarVisualPtr lidar = this->CreateLidarVisualImpl(objId, objName); + bool result = this->RegisterVisual(lidar); + return (result) ? lidar : nullptr; } ////////////////////////////////////////////////// From fc482461e4e10c909bf7ba840b05bcdee2811112 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 20 Jul 2020 13:19:10 +0530 Subject: [PATCH 12/17] Minor fixes Signed-off-by: Mihir Kulkarni --- .../rendering/ogre/OgreLidarVisual.hh | 2 +- test/integration/lidar_visual.cc | 26 +++++++++++++++++++ 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh b/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh index ab44addd3..de93f234f 100644 --- a/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh +++ b/ogre/include/ignition/rendering/ogre/OgreLidarVisual.hh @@ -51,7 +51,7 @@ namespace ignition public: virtual void PreRender() override; // Documentation inherited. - public: virtual void Destroy(); + public: virtual void Destroy() override; // Documentation inherited public: virtual void Update() override; diff --git a/test/integration/lidar_visual.cc b/test/integration/lidar_visual.cc index e8b87c681..8c3d6a964 100644 --- a/test/integration/lidar_visual.cc +++ b/test/integration/lidar_visual.cc @@ -133,6 +133,26 @@ void LidarVisualTest::Configure(const std::string &_renderEngine) ignition::math::Pose3d offset(1.5, 3.6, 2.9, 1.1, -5.3, -2.9); lidarVis->SetOffset(offset); EXPECT_EQ(lidarVis->Offset(), offset); + + lidarVis->SetType(LVT_NONE); + EXPECT_EQ(lidarVis->Type(), LVT_NONE); + lidarVis->SetType(LVT_POINTS); + EXPECT_EQ(lidarVis->Type(), LVT_POINTS); + lidarVis->SetType(LVT_POINTS); + EXPECT_EQ(lidarVis->Type(), LVT_POINTS); + lidarVis->SetType(LVT_TRIANGLE_STRIPS); + EXPECT_EQ(lidarVis->Type(), LVT_TRIANGLE_STRIPS); + + lidarVis->SetDisplayNonHitting(true); + EXPECT_EQ(lidarVis->DisplayNonHitting(), true); + lidarVis->SetDisplayNonHitting(false); + EXPECT_EQ(lidarVis->DisplayNonHitting(), false); + + std::vector pts = {2, 14, 15, 3, 5, 10, 3}; + lidarVis->SetPoints(pts); + EXPECT_EQ(lidarVis->PointCount(), pts.size()); + lidarVis->ClearPoints(); + EXPECT_EQ(lidarVis->PointCount(), 0u); } // Clean up @@ -216,6 +236,8 @@ void LidarVisualTest::RaysUnitBox(const std::string &_renderEngine) lidarVis->SetMaxHorizontalAngle(hMaxAngle); lidarVis->SetHorizontalRayCount(hRayCount); lidarVis->SetVerticalRayCount(vRayCount); + lidarVis->SetType(LidarVisualType::LVT_TRIANGLE_STRIPS); + lidarVis->SetDisplayNonHitting(true); root->AddChild(lidarVis); // Create a second ray caster rotated @@ -244,6 +266,8 @@ void LidarVisualTest::RaysUnitBox(const std::string &_renderEngine) lidarVis2->SetMaxHorizontalAngle(hMaxAngle); lidarVis2->SetHorizontalRayCount(hRayCount); lidarVis2->SetVerticalRayCount(vRayCount); + lidarVis2->SetType(LidarVisualType::LVT_TRIANGLE_STRIPS); + lidarVis2->SetDisplayNonHitting(true); root->AddChild(lidarVis2); // Create testing boxes @@ -463,6 +487,8 @@ void LidarVisualTest::LaserVertical(const std::string &_renderEngine) lidarVis->SetMaxHorizontalAngle(hMaxAngle); lidarVis->SetHorizontalRayCount(hRayCount); lidarVis->SetVerticalRayCount(vRayCount); + lidarVis->SetType(LidarVisualType::LVT_TRIANGLE_STRIPS); + lidarVis->SetDisplayNonHitting(true); root->AddChild(lidarVis); // Create testing boxes From 3296d4ca462da56400c7dcfe9f6d76daa4df245a Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Mon, 20 Jul 2020 13:48:34 +0530 Subject: [PATCH 13/17] Can set id and name of lidar visual Signed-off-by: Mihir Kulkarni --- include/ignition/rendering/Scene.hh | 28 ++++++++++++++++++-- include/ignition/rendering/base/BaseScene.hh | 12 +++++++++ src/base/BaseScene.cc | 24 +++++++++++++++-- 3 files changed, 60 insertions(+), 4 deletions(-) diff --git a/include/ignition/rendering/Scene.hh b/include/ignition/rendering/Scene.hh index e2165075f..5cf7299af 100644 --- a/include/ignition/rendering/Scene.hh +++ b/include/ignition/rendering/Scene.hh @@ -855,10 +855,34 @@ namespace ignition /// \return The created marker public: virtual MarkerPtr CreateMarker() = 0; - // /// \brief Create new lidar visual. - // /// \return The created lidar visual + /// \brief Create new lidar visual. A unique ID and name will + /// automatically be assigned to the lidar visual. + /// \return The created lidar visual public: virtual LidarVisualPtr CreateLidarVisual() = 0; + /// \brief Create new lidar visual with the given ID. A unique name + /// will automatically be assigned to the lidar visual. If the given + /// ID is already in use, NULL will be returned. + /// \param[in] _id ID of the new lidar visual + /// \return The created lidar visual + public: virtual LidarVisualPtr CreateLidarVisual(unsigned int _id) = 0; + + /// \brief Create new lidar visual with the given name. A unique ID + /// will automatically be assigned to the lidar visual. If the given + /// name is already in use, NULL will be returned. + /// \param[in] _name Name of the new lidar visual + /// \return The created lidar visual + public: virtual LidarVisualPtr CreateLidarVisual( + const std::string &_name) = 0; + + /// \brief Create new lidar visual with the given name. If either + /// the given ID or name is already in use, NULL will be returned. + /// \param[in] _id ID of the lidar visual. + /// \param[in] _name Name of the new lidar visual. + /// \return The created lidar visual + public: virtual LidarVisualPtr CreateLidarVisual( + unsigned int _id, const std::string &_name) = 0; + /// \brief Create new text geometry. /// \return The created text public: virtual TextPtr CreateText() = 0; diff --git a/include/ignition/rendering/base/BaseScene.hh b/include/ignition/rendering/base/BaseScene.hh index e786913b4..913f3fab0 100644 --- a/include/ignition/rendering/base/BaseScene.hh +++ b/include/ignition/rendering/base/BaseScene.hh @@ -379,6 +379,18 @@ namespace ignition // Documentation inherited. public: virtual LidarVisualPtr CreateLidarVisual() override; + // Documentation inherited. + public: virtual LidarVisualPtr CreateLidarVisual( + unsigned int _id) override; + + // Documentation inherited. + public: virtual LidarVisualPtr CreateLidarVisual( + const std::string &_name) override; + + // Documentation inherited. + public: virtual LidarVisualPtr CreateLidarVisual(unsigned int _id, + const std::string &_name) override; + // Documentation inherited. public: virtual WireBoxPtr CreateWireBox() override; diff --git a/src/base/BaseScene.cc b/src/base/BaseScene.cc index f2089c806..623abb23b 100644 --- a/src/base/BaseScene.cc +++ b/src/base/BaseScene.cc @@ -991,8 +991,28 @@ MarkerPtr BaseScene::CreateMarker() LidarVisualPtr BaseScene::CreateLidarVisual() { unsigned int objId = this->CreateObjectId(); - const std::string objName = this->CreateObjectName(objId, "LidarVisual"); - LidarVisualPtr lidar = this->CreateLidarVisualImpl(objId, objName); + return this->CreateLidarVisual(objId); +} + +////////////////////////////////////////////////// +LidarVisualPtr BaseScene::CreateLidarVisual(unsigned int _id) +{ + const std::string objName = this->CreateObjectName(_id, "LidarVisual"); + return this->CreateLidarVisual(_id, objName); +} + +////////////////////////////////////////////////// +LidarVisualPtr BaseScene::CreateLidarVisual(const std::string &_name) +{ + unsigned int objId = this->CreateObjectId(); + return this->CreateLidarVisual(objId, _name); +} + +////////////////////////////////////////////////// +LidarVisualPtr BaseScene::CreateLidarVisual(unsigned int _id, + const std::string &_name) +{ + LidarVisualPtr lidar = this->CreateLidarVisualImpl(_id, _name); bool result = this->RegisterVisual(lidar); return (result) ? lidar : nullptr; } From 4ab331798951b6fd25b64b52846e066efd3d7b1e Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 21 Jul 2020 21:49:42 +0530 Subject: [PATCH 14/17] Remove space MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Alejandro Hernández Cordero Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/GlutWindow.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/lidar_visual/GlutWindow.cc b/examples/lidar_visual/GlutWindow.cc index 74bcac392..bf0d9b340 100644 --- a/examples/lidar_visual/GlutWindow.cc +++ b/examples/lidar_visual/GlutWindow.cc @@ -257,7 +257,7 @@ void updateLidarVisual() g_time = std::chrono::steady_clock::now() - g_startTime; prevUpdateTime = std::chrono::duration_cast( - g_time).count(); + g_time).count(); } } From 14f68a1c18588cb62568dc061f0401b262e2748c Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 21 Jul 2020 21:53:44 +0530 Subject: [PATCH 15/17] removed whitespaces Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/GlutWindow.cc | 7 ------- examples/lidar_visual/Main.cc | 1 - 2 files changed, 8 deletions(-) diff --git a/examples/lidar_visual/GlutWindow.cc b/examples/lidar_visual/GlutWindow.cc index bf0d9b340..78dd83144 100644 --- a/examples/lidar_visual/GlutWindow.cc +++ b/examples/lidar_visual/GlutWindow.cc @@ -261,7 +261,6 @@ void updateLidarVisual() } } - ////////////////////////////////////////////////// void drawText(int _x, int _y, const std::string &_text) { @@ -283,7 +282,6 @@ void drawText(int _x, int _y, const std::string &_text) glPopMatrix(); } - ////////////////////////////////////////////////// void displayCB() { @@ -320,7 +318,6 @@ void displayCB() text << std::setw(30) << "Update time (microseconds): " << prevUpdateTime; drawText(10, 10, text.str()); - glutSwapBuffers(); } @@ -376,7 +373,6 @@ void idleCB() glutPostRedisplay(); } - ////////////////////////////////////////////////// void initCamera(ir::CameraPtr _camera) { @@ -388,14 +384,12 @@ void initCamera(ir::CameraPtr _camera) g_camera->Capture(*g_image); } - ////////////////////////////////////////////////// void initLidarVisual(ir::LidarVisualPtr _lidar) { g_lidar = _lidar; } - ////////////////////////////////////////////////// void initContext() { @@ -410,7 +404,6 @@ void initContext() glutMotionFunc(motionCB); } - ////////////////////////////////////////////////// void printUsage() { diff --git a/examples/lidar_visual/Main.cc b/examples/lidar_visual/Main.cc index 2b0b51922..56b067e9b 100644 --- a/examples/lidar_visual/Main.cc +++ b/examples/lidar_visual/Main.cc @@ -36,7 +36,6 @@ using namespace ignition; using namespace rendering; - const std::string RESOURCE_PATH = common::joinPaths(std::string(PROJECT_BINARY_PATH), "media"); From fa37115fddf09c12a3392a17c2651ec698fee6ee Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Tue, 21 Jul 2020 21:59:32 +0530 Subject: [PATCH 16/17] fixed document arguments Signed-off-by: Mihir Kulkarni --- examples/lidar_visual/GlutWindow.hh | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/lidar_visual/GlutWindow.hh b/examples/lidar_visual/GlutWindow.hh index c778e154f..ea7d26a46 100644 --- a/examples/lidar_visual/GlutWindow.hh +++ b/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 +/// \param[in] nodes Nodes in the scene +/// \param[in] _pts Lidar points void run(std::vector _cameras, std::vector _nodes, std::vector _pts); From 9be898ea7844934a36806d5366ea77bab9f26921 Mon Sep 17 00:00:00 2001 From: Mihir Kulkarni Date: Wed, 22 Jul 2020 07:20:35 +0530 Subject: [PATCH 17/17] fix windows CI build error Signed-off-by: Mihir Kulkarni --- ogre/src/OgreLidarVisual.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ogre/src/OgreLidarVisual.cc b/ogre/src/OgreLidarVisual.cc index 0776c1cfe..5e8d6145c 100644 --- a/ogre/src/OgreLidarVisual.cc +++ b/ogre/src/OgreLidarVisual.cc @@ -300,8 +300,9 @@ void OgreLidarVisual::Update() #if (OGRE_VERSION <= ((1 << 16) | (10 << 8) | 7)) line->setMaterial("Lidar/BlueRay"); #else - mat = Ogre::MaterialManager::getSingleton().getByName( - "Lidar/BlueRay"); + Ogre::MaterialPtr mat = + Ogre::MaterialManager::getSingleton().getByName( + "Lidar/BlueRay"); line->setMaterial(mat); #endif std::shared_ptr mv =