From 15ecaf3ab47cbb39550c025940570cd305ac40a7 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 29 May 2024 18:57:08 +0200 Subject: [PATCH 01/10] Migrate CMakeLists to work with gz harmonic --- RGLServerPlugin/CMakeLists.txt | 17 +++++++++++------ RGLVisualize/CMakeLists.txt | 20 +++++++++++++------- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/RGLServerPlugin/CMakeLists.txt b/RGLServerPlugin/CMakeLists.txt index eef7db9..642d1e8 100644 --- a/RGLServerPlugin/CMakeLists.txt +++ b/RGLServerPlugin/CMakeLists.txt @@ -2,8 +2,13 @@ cmake_minimum_required(VERSION 3.16) project(RGLServerPlugin) set(CMAKE_CXX_STANDARD 20) -find_package(ignition-gazebo6 REQUIRED) -find_package(ignition-plugin1 REQUIRED) +find_package(gz-cmake3 REQUIRED) +gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) + +gz_find_package(gz-sim8 REQUIRED) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + include_directories(include) @@ -29,16 +34,16 @@ set(RobotecGPULidar ${RGL_SO_PATH}) add_library(RGLServerPluginManager SHARED src/RGLServerPluginManager.cc src/Mesh.cc src/Utils.cc src/Scene.cc) target_link_libraries(RGLServerPluginManager - ignition-gazebo6::ignition-gazebo6 - ignition-plugin1::ignition-plugin1 + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ${RobotecGPULidar} ) set_target_properties(RGLServerPluginManager PROPERTIES INSTALL_RPATH "$ORIGIN") add_library(RGLServerPluginInstance SHARED src/RGLServerPluginInstance.cc src/Lidar.cc src/Utils.cc src/LidarPatternLoader.cc) target_link_libraries(RGLServerPluginInstance - ignition-gazebo6::ignition-gazebo6 - ignition-plugin1::ignition-plugin1 + PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} + PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} ${RobotecGPULidar} ) set_target_properties(RGLServerPluginInstance PROPERTIES INSTALL_RPATH "$ORIGIN") diff --git a/RGLVisualize/CMakeLists.txt b/RGLVisualize/CMakeLists.txt index 35dbaca..0b22237 100644 --- a/RGLVisualize/CMakeLists.txt +++ b/RGLVisualize/CMakeLists.txt @@ -7,12 +7,18 @@ endif() project(RGLVisualize) -find_package(ignition-rendering6 REQUIRED) - set(CMAKE_AUTOMOC ON) -find_package(ignition-gui6 REQUIRED) -find_package(ignition-msgs8 REQUIRED) +find_package(gz-cmake3 REQUIRED) +gz_find_package(gz-msgs10 REQUIRED) +set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + +gz_find_package(gz-rendering8 REQUIRED) +set(GZ_RENDERING_VER ${gz-rendering8_VERSION_MAJOR}) + + +gz_find_package(gz-gui8 REQUIRED) +set(GZ_GUI_VER ${gz-gui8_VERSION_MAJOR}) QT5_ADD_RESOURCES(resources_RCC RGLVisualize.qrc) @@ -23,9 +29,9 @@ add_library(RGLVisualize SHARED target_link_libraries(RGLVisualize PRIVATE - ignition-gui6::ignition-gui6 - ignition-rendering6::ignition-rendering6 - ignition-msgs8::ignition-msgs8 + gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} + gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER} ) ## Install libraries From 5c66246ef0fcf907b2e4562a7fc0cd4bb6040daf Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 29 May 2024 18:58:01 +0200 Subject: [PATCH 02/10] Migrate source code to work with gz harmonic --- RGLServerPlugin/include/LidarPatternLoader.hh | 11 ++-- .../include/RGLServerPluginInstance.hh | 58 +++++++++-------- .../include/RGLServerPluginManager.hh | 64 +++++++++---------- RGLServerPlugin/include/Utils.hh | 16 ++--- RGLServerPlugin/src/Lidar.cc | 44 ++++++------- RGLServerPlugin/src/LidarPatternLoader.cc | 45 ++++++------- RGLServerPlugin/src/Mesh.cc | 18 +++--- .../src/RGLServerPluginInstance.cc | 24 +++---- RGLServerPlugin/src/RGLServerPluginManager.cc | 27 ++++---- RGLServerPlugin/src/Scene.cc | 42 ++++++------ RGLServerPlugin/src/Utils.cc | 27 ++++---- RGLVisualize/RGLVisualize.cc | 59 ++++++++--------- RGLVisualize/RGLVisualize.hh | 14 ++-- RGLVisualize/RGLVisualize.qml | 2 +- 14 files changed, 228 insertions(+), 223 deletions(-) diff --git a/RGLServerPlugin/include/LidarPatternLoader.hh b/RGLServerPlugin/include/LidarPatternLoader.hh index 4b41986..73c9a5b 100644 --- a/RGLServerPlugin/include/LidarPatternLoader.hh +++ b/RGLServerPlugin/include/LidarPatternLoader.hh @@ -19,8 +19,7 @@ #include -#include - +#include namespace rgl { @@ -35,7 +34,7 @@ private: LidarPatternLoader() {}; static bool LoadAnglesAndSamplesElement(const sdf::ElementConstPtr& sdf, - ignition::math::Angle& angleMin, ignition::math::Angle& angleMax, + gz::math::Angle& angleMin, gz::math::Angle& angleMax, int& samples); static bool LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector& outPattern); @@ -44,9 +43,9 @@ private: static bool LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern); static bool LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& outPattern); - static rgl_mat3x4f AnglesToRglMat3x4f(const ignition::math::Angle& roll, - const ignition::math::Angle& pitch, - const ignition::math::Angle& yaw); + static rgl_mat3x4f AnglesToRglMat3x4f(const gz::math::Angle& roll, + const gz::math::Angle& pitch, + const gz::math::Angle& yaw); template static std::vector LoadVector(const std::filesystem::path& path); diff --git a/RGLServerPlugin/include/RGLServerPluginInstance.hh b/RGLServerPlugin/include/RGLServerPluginInstance.hh index 6f7cab7..d73e38d 100644 --- a/RGLServerPlugin/include/RGLServerPluginInstance.hh +++ b/RGLServerPlugin/include/RGLServerPluginInstance.hh @@ -19,22 +19,24 @@ #include "rgl/api/core.h" #include "LidarPatternLoader.hh" -#include +#include -#include -#include -#include +#include +#include +#include + +#include +#include -#include namespace rgl { class RGLServerPluginInstance : - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPreUpdate, - public ignition::gazebo::ISystemPostUpdate + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { public: RGLServerPluginInstance() = default; @@ -43,42 +45,42 @@ public: // only called once, when plugin is being loaded void Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) override; // called every time before physics update runs (can change entities) void PreUpdate( - const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) override; + const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) override; // called every time after physics runs (can't change entities) void PostUpdate( - const ignition::gazebo::UpdateInfo& info, - const ignition::gazebo::EntityComponentManager& ecm) override; + const gz::sim::UpdateInfo& info, + const gz::sim::EntityComponentManager& ecm) override; private: bool LoadConfiguration(const std::shared_ptr& sdf); - void CreateLidar(ignition::gazebo::Entity entity, - ignition::gazebo::EntityComponentManager& ecm); + void CreateLidar(gz::sim::Entity entity, + gz::sim::EntityComponentManager& ecm); - void UpdateLidarPose(const ignition::gazebo::EntityComponentManager& ecm); + void UpdateLidarPose(const gz::sim::EntityComponentManager& ecm); bool ShouldRayTrace(std::chrono::steady_clock::duration sim_time, bool paused); void RayTrace(std::chrono::steady_clock::duration sim_time); - ignition::msgs::PointCloudPacked CreatePointCloudMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount); - ignition::msgs::LaserScan CreateLaserScanMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount); + gz::msgs::PointCloudPacked CreatePointCloudMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount); + gz::msgs::LaserScan CreateLaserScanMsg(std::chrono::steady_clock::duration sim_time, std::string frame, int hitpointCount); void DestroyLidar(); std::string topicName; std::string frameId; float lidarRange; - ignition::math::Angle scanHMin; - ignition::math::Angle scanHMax; + gz::math::Angle scanHMin; + gz::math::Angle scanHMax; int scanHSamples; std::vector lidarPattern; std::vector resultPointCloud; @@ -87,11 +89,11 @@ private: bool updateOnPausedSim = false; bool publishLaserScan = false; - ignition::gazebo::Entity thisLidarEntity; - ignition::transport::Node::Publisher pointCloudPublisher; - ignition::transport::Node::Publisher laserScanPublisher; - ignition::transport::Node::Publisher pointCloudWorldPublisher; - ignition::transport::Node gazeboNode; + gz::sim::Entity thisLidarEntity; + gz::transport::Node::Publisher pointCloudPublisher; + gz::transport::Node::Publisher laserScanPublisher; + gz::transport::Node::Publisher pointCloudWorldPublisher; + gz::transport::Node gazeboNode; rgl_node_t rglNodeUseRays = nullptr; rgl_node_t rglNodeLidarPose = nullptr; diff --git a/RGLServerPlugin/include/RGLServerPluginManager.hh b/RGLServerPlugin/include/RGLServerPluginManager.hh index 4dca048..2a34224 100644 --- a/RGLServerPlugin/include/RGLServerPluginManager.hh +++ b/RGLServerPlugin/include/RGLServerPluginManager.hh @@ -18,84 +18,84 @@ #include "Utils.hh" -#include +#include -#include -#include -#include +#include +#include +#include -#include +#include namespace rgl { class RGLServerPluginManager : - public ignition::gazebo::System, - public ignition::gazebo::ISystemConfigure, - public ignition::gazebo::ISystemPostUpdate + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate { public: - using MeshInfo = std::variant; + using MeshInfo = std::variant; RGLServerPluginManager() = default; ~RGLServerPluginManager() override = default; // only called once, when plugin is being loaded void Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& eventMgr) override; + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& eventMgr) override; // called every time after physics runs (can't change entities) void PostUpdate( - const ignition::gazebo::UpdateInfo& info, - const ignition::gazebo::EntityComponentManager& ecm) override; + const gz::sim::UpdateInfo& info, + const gz::sim::EntityComponentManager& ecm) override; private: ////////////////////////////////////////////// Variables ///////////////////////////////////////////// ////////////////////////////// Lidar //////////////////////////////// // contains pointers to all entities that were loaded to rgl (as well as to their meshes) - std::unordered_map> entitiesInRgl; + std::unordered_map> entitiesInRgl; // the entity ids, that the lidars are attached to - std::unordered_set lidarEntities; + std::unordered_set lidarEntities; // all entities, that the lidar should ignore - std::unordered_set entitiesToIgnore; + std::unordered_set entitiesToIgnore; ////////////////////////////// Mesh ///////////////////////////////// - ignition::common::MeshManager* meshManager{ignition::common::MeshManager::Instance()}; + gz::common::MeshManager* meshManager{gz::common::MeshManager::Instance()}; ////////////////////////////////////////////// Functions ///////////////////////////////////////////// ////////////////////////////// Scene //////////////////////////////// bool RegisterNewLidarCb( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm); + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm); bool UnregisterLidarCb( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm); + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm); bool LoadEntityToRGLCb( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::components::Visual*, - const ignition::gazebo::components::Geometry* geometry); + const gz::sim::Entity& entity, + const gz::sim::components::Visual*, + const gz::sim::components::Geometry* geometry); bool RemoveEntityFromRGLCb( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::components::Visual*, - const ignition::gazebo::components::Geometry*); + const gz::sim::Entity& entity, + const gz::sim::components::Visual*, + const gz::sim::components::Geometry*); - void UpdateRGLEntityPoses(const ignition::gazebo::EntityComponentManager& ecm); + void UpdateRGLEntityPoses(const gz::sim::EntityComponentManager& ecm); - std::unordered_set GetEntitiesInParentLink( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm); + std::unordered_set GetEntitiesInParentLink( + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm); ////////////////////////////// Mesh ///////////////////////////////// diff --git a/RGLServerPlugin/include/Utils.hh b/RGLServerPlugin/include/Utils.hh index 619ee10..ee97519 100644 --- a/RGLServerPlugin/include/Utils.hh +++ b/RGLServerPlugin/include/Utils.hh @@ -16,8 +16,8 @@ #include "rgl/api/core.h" -#include -#include +#include +#include namespace rgl { @@ -27,15 +27,15 @@ namespace rgl // When API call returns error status, it prints error string and returns false. bool CheckRGL(rgl_status_t status); -ignition::math::Pose3 FindWorldPose( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::EntityComponentManager& ecm); +gz::math::Pose3 FindWorldPose( + const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm); rgl_mat3x4f FindWorldPoseInRglMatrix( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::EntityComponentManager& ecm); + const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm); -rgl_mat3x4f IgnPose3dToRglMatrix(const ignition::math::Pose3& pose); +rgl_mat3x4f IgnPose3dToRglMatrix(const gz::math::Pose3& pose); // Throws exception when version of RGL library mismatch diff --git a/RGLServerPlugin/src/Lidar.cc b/RGLServerPlugin/src/Lidar.cc index 4dcae89..ec14482 100644 --- a/RGLServerPlugin/src/Lidar.cc +++ b/RGLServerPlugin/src/Lidar.cc @@ -84,8 +84,8 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptr(topicName); + pointCloudPublisher = gazeboNode.Advertise(topicName); } else { @@ -137,18 +137,18 @@ void RGLServerPluginInstance::CreateLidar(ignition::gazebo::Entity entity, ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; } ignmsg << "Start publishing LaserScan messages on topic '" << topicName << "'\n"; - laserScanPublisher = gazeboNode.Advertise(topicName); + laserScanPublisher = gazeboNode.Advertise(topicName); } - pointCloudWorldPublisher = gazeboNode.Advertise(topicName + worldTopicPostfix); + pointCloudWorldPublisher = gazeboNode.Advertise(topicName + worldTopicPostfix); isLidarInitialized = true; } -void RGLServerPluginInstance::UpdateLidarPose(const ignition::gazebo::EntityComponentManager& ecm) +void RGLServerPluginInstance::UpdateLidarPose(const gz::sim::EntityComponentManager& ecm) { - ignition::math::Pose3 ignLidarToWorld = FindWorldPose(thisLidarEntity, ecm); - ignition::math::Pose3 ignWorldToLidar = ignLidarToWorld.Inverse(); + gz::math::Pose3 ignLidarToWorld = FindWorldPose(thisLidarEntity, ecm); + gz::math::Pose3 ignWorldToLidar = ignLidarToWorld.Inverse(); rgl_mat3x4f rglLidarToWorld = IgnPose3dToRglMatrix(ignLidarToWorld); rgl_mat3x4f rglWorldToLidar = IgnPose3dToRglMatrix(ignWorldToLidar); CheckRGL(rgl_node_rays_transform(&rglNodeLidarPose, &rglLidarToWorld)); @@ -228,10 +228,10 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi } } -ignition::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) +gz::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) { - ignition::msgs::LaserScan outMsg; - *outMsg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(simTime); + gz::msgs::LaserScan outMsg; + *outMsg.mutable_header()->mutable_stamp() = gz::msgs::Convert(simTime); auto _frame = outMsg.mutable_header()->add_data(); _frame->set_key("frame_id"); _frame->add_value(frame); @@ -242,7 +242,7 @@ ignition::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chron outMsg.set_range_min(0.0); outMsg.set_range_max(lidarRange); - ignition::math::Angle hStep((scanHMax-scanHMin)/scanHSamples); + gz::math::Angle hStep((scanHMax-scanHMin)/scanHSamples); outMsg.set_angle_min(scanHMin.Radian()); outMsg.set_angle_max(scanHMax.Radian()); @@ -258,17 +258,17 @@ ignition::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chron return outMsg; } -ignition::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) +gz::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) { - ignition::msgs::PointCloudPacked outMsg; - ignition::msgs::InitPointCloudPacked(outMsg, frame, false, - {{"xyz", ignition::msgs::PointCloudPacked::Field::FLOAT32}}); + gz::msgs::PointCloudPacked outMsg; + gz::msgs::InitPointCloudPacked(outMsg, frame, false, + {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); outMsg.mutable_data()->resize(hitpointCount * outMsg.point_step()); - *outMsg.mutable_header()->mutable_stamp() = ignition::msgs::Convert(simTime); + *outMsg.mutable_header()->mutable_stamp() = gz::msgs::Convert(simTime); outMsg.set_height(1); outMsg.set_width(hitpointCount); - ignition::msgs::PointCloudPackedIterator xIterWorld(outMsg, "x"); + gz::msgs::PointCloudPackedIterator xIterWorld(outMsg, "x"); memcpy(&(*xIterWorld), resultPointCloud.data(), hitpointCount * sizeof(rgl_vec3f)); return outMsg; } @@ -284,11 +284,11 @@ void RGLServerPluginInstance::DestroyLidar() } // Reset publishers if (!publishLaserScan) { - pointCloudPublisher = ignition::transport::Node::Publisher(); + pointCloudPublisher = gz::transport::Node::Publisher(); } else { - laserScanPublisher = ignition::transport::Node::Publisher(); + laserScanPublisher = gz::transport::Node::Publisher(); } - pointCloudWorldPublisher = ignition::transport::Node::Publisher(); + pointCloudWorldPublisher = gz::transport::Node::Publisher(); isLidarInitialized = false; } diff --git a/RGLServerPlugin/src/LidarPatternLoader.cc b/RGLServerPlugin/src/LidarPatternLoader.cc index 926b805..debe4cc 100644 --- a/RGLServerPlugin/src/LidarPatternLoader.cc +++ b/RGLServerPlugin/src/LidarPatternLoader.cc @@ -15,6 +15,7 @@ #include #include "LidarPatternLoader.hh" +#include "gz/math/Matrix4.hh" #define PATTERNS_DIR_ENV "RGL_PATTERNS_DIR" @@ -59,7 +60,7 @@ bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vectorHasElement("samples")) { @@ -106,7 +107,7 @@ bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, return false; } - ignition::math::Angle vMin, vMax, hMin, hMax; + gz::math::Angle vMin, vMax, hMin, hMax; int vSamples, hSamples; if (!LoadAnglesAndSamplesElement(sdf->FindElement("vertical"), vMin, vMax, vSamples)) { @@ -119,17 +120,17 @@ bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, outPattern.reserve(vSamples * hSamples); - ignition::math::Angle vStep((vMax - vMin) / static_cast(vSamples)); - ignition::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); + gz::math::Angle vStep((vMax - vMin) / static_cast(vSamples)); + gz::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); auto vAngle = vMin; for (int i = 0; i < vSamples; ++i) { auto hAngle = hMin; for (int j = 0; j < hSamples; ++j) { outPattern.push_back( - AnglesToRglMat3x4f(ignition::math::Angle::Zero, + AnglesToRglMat3x4f(gz::math::Angle::Zero, // Inverse and shift 90deg pitch to match uniform pattern from Gazebo - vAngle * -1 + ignition::math::Angle::HalfPi, + vAngle * -1 + gz::math::Angle::HalfPi, hAngle)); hAngle += hStep; } @@ -147,10 +148,10 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, auto channelAngles = sdf->GetAttribute("channels"); - std::vector channels; + std::vector channels; std::istringstream iss(channelAngles->GetAsString()); - std::copy(std::istream_iterator(iss), - std::istream_iterator(), + std::copy(std::istream_iterator(iss), + std::istream_iterator(), std::back_inserter(channels)); if (channels.empty()) { @@ -158,13 +159,13 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, return false; } - ignition::math::Angle hMin, hMax; + gz::math::Angle hMin, hMax; int hSamples; if (!LoadAnglesAndSamplesElement(sdf->FindElement("horizontal"), hMin, hMax, hSamples)) { return false; } - ignition::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); + gz::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); outPattern.reserve(channels.size() * hSamples); @@ -172,9 +173,9 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, auto hAngle = hMin; for (int j = 0; j < hSamples; ++j) { outPattern.push_back( - AnglesToRglMat3x4f(ignition::math::Angle::Zero, + AnglesToRglMat3x4f(gz::math::Angle::Zero, // Inverse and shift 90deg pitch to match uniform pattern from Gazebo - channel * -1 + ignition::math::Angle::HalfPi, + channel * -1 + gz::math::Angle::HalfPi, hAngle)); hAngle += hStep; } @@ -221,7 +222,7 @@ bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, return false; } - ignition::math::Angle hMin, hMax; + gz::math::Angle hMin, hMax; int hSamples; if (!LoadAnglesAndSamplesElement(sdf->FindElement("horizontal"), hMin, hMax, hSamples)) { @@ -231,14 +232,14 @@ bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, outPattern.clear(); outPattern.reserve(hSamples); - ignition::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); + gz::math::Angle hStep((hMax - hMin) / static_cast(hSamples)); auto hAngle = hMin; for (int i = 0; i < hSamples; ++i) { outPattern.push_back( - AnglesToRglMat3x4f(ignition::math::Angle::Zero, + AnglesToRglMat3x4f(gz::math::Angle::Zero, // Inverse and shift 90deg pitch to match uniform pattern from Gazebo - ignition::math::Angle::HalfPi, + gz::math::Angle::HalfPi, hAngle)); hAngle += hStep; } @@ -274,12 +275,12 @@ std::vector LidarPatternLoader::LoadVector(const fs::path& path) return fileData; } -rgl_mat3x4f LidarPatternLoader::AnglesToRglMat3x4f(const ignition::math::Angle& roll, - const ignition::math::Angle& pitch, - const ignition::math::Angle& yaw) +rgl_mat3x4f LidarPatternLoader::AnglesToRglMat3x4f(const gz::math::Angle& roll, + const gz::math::Angle& pitch, + const gz::math::Angle& yaw) { - ignition::math::Quaterniond quaternion(roll.Radian(), pitch.Radian(), yaw.Radian()); - ignition::math::Matrix4d matrix4D(quaternion); + gz::math::Quaterniond quaternion(roll.Radian(), pitch.Radian(), yaw.Radian()); + gz::math::Matrix4d matrix4D(quaternion); rgl_mat3x4f rglMatrix; for (int i = 0; i < 3; ++i) { diff --git a/RGLServerPlugin/src/Mesh.cc b/RGLServerPlugin/src/Mesh.cc index 0b272fc..735448e 100644 --- a/RGLServerPlugin/src/Mesh.cc +++ b/RGLServerPlugin/src/Mesh.cc @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include #include @@ -118,11 +118,11 @@ RGLServerPluginManager::MeshInfo RGLServerPluginManager::LoadMesh( scaleY = scale.Y(); scaleZ = scale.Z(); - std::string meshPath = ignition::gazebo::asFullPath( + std::string meshPath = gz::sim::asFullPath( data.MeshShape()->Uri(), data.MeshShape()->FilePath()); - const ignition::common::Mesh* mesh = meshManager->Load(meshPath); + const gz::common::Mesh* mesh = meshManager->Load(meshPath); if (mesh == nullptr) { ignerr << "Failed to import mesh to RGL: " << meshPath << ".\n"; @@ -138,7 +138,7 @@ RGLServerPluginManager::MeshInfo RGLServerPluginManager::LoadMesh( if (auto subMesh = mesh->SubMeshByName(subMeshName).lock()) { // subMesh must not be null if (subMesh.get()) { - ignition::common::SubMesh subMeshCopy(*subMesh); + gz::common::SubMesh subMeshCopy(*subMesh); if (data.MeshShape()->CenterSubmesh()) { subMeshCopy.Center(); } @@ -226,14 +226,14 @@ bool RGLServerPluginManager::LoadMeshToRGL( std::vector rglVertices; // separated array because ign operates on doubles, and rgl on floats rgl_vec3i* triangles = nullptr; - if (std::holds_alternative(meshInfo)) { - auto ignSubMesh = get(meshInfo); + if (std::holds_alternative(meshInfo)) { + auto ignSubMesh = get(meshInfo); vertexCount = static_cast(ignSubMesh.VertexCount()); triangleCount = static_cast(ignSubMesh.IndexCount() / 3); rglVertices.reserve(vertexCount); ignSubMesh.FillArrays(&ignVertices, reinterpret_cast(&triangles)); } else { - auto ignMesh = get(meshInfo); + auto ignMesh = get(meshInfo); vertexCount = static_cast(ignMesh->VertexCount()); triangleCount = static_cast(ignMesh->IndexCount() / 3); rglVertices.reserve(vertexCount); diff --git a/RGLServerPlugin/src/RGLServerPluginInstance.cc b/RGLServerPlugin/src/RGLServerPluginInstance.cc index 93eb41b..a53ae63 100644 --- a/RGLServerPlugin/src/RGLServerPluginInstance.cc +++ b/RGLServerPlugin/src/RGLServerPluginInstance.cc @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include "RGLServerPluginInstance.hh" -IGNITION_ADD_PLUGIN( +GZ_ADD_PLUGIN( rgl::RGLServerPluginInstance, - ignition::gazebo::System, + gz::sim::System, rgl::RGLServerPluginInstance::ISystemConfigure, rgl::RGLServerPluginInstance::ISystemPreUpdate, rgl::RGLServerPluginInstance::ISystemPostUpdate @@ -31,10 +31,10 @@ namespace rgl { void RGLServerPluginInstance::Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr& sdf, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager&) + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager&) { if (LoadConfiguration(sdf)) { CreateLidar(entity, ecm); @@ -42,8 +42,8 @@ void RGLServerPluginInstance::Configure( } void RGLServerPluginInstance::PreUpdate( - const ignition::gazebo::UpdateInfo& info, - ignition::gazebo::EntityComponentManager& ecm) + const gz::sim::UpdateInfo& info, + gz::sim::EntityComponentManager& ecm) { if (ShouldRayTrace(info.simTime, info.paused)) { UpdateLidarPose(ecm); @@ -52,10 +52,10 @@ void RGLServerPluginInstance::PreUpdate( } void RGLServerPluginInstance::PostUpdate( - const ignition::gazebo::UpdateInfo& info, - const ignition::gazebo::EntityComponentManager& ecm) + const gz::sim::UpdateInfo& info, + const gz::sim::EntityComponentManager& ecm) { - ecm.EachRemoved<>([&](const ignition::gazebo::Entity& entity)-> bool { + ecm.EachRemoved<>([&](const gz::sim::Entity& entity)-> bool { if (entity == thisLidarEntity) { DestroyLidar(); } diff --git a/RGLServerPlugin/src/RGLServerPluginManager.cc b/RGLServerPlugin/src/RGLServerPluginManager.cc index d7bd76f..3507d7d 100644 --- a/RGLServerPlugin/src/RGLServerPluginManager.cc +++ b/RGLServerPlugin/src/RGLServerPluginManager.cc @@ -12,14 +12,15 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include +#include +#include #include "RGLServerPluginManager.hh" -IGNITION_ADD_PLUGIN( + +GZ_ADD_PLUGIN( rgl::RGLServerPluginManager, - ignition::gazebo::System, + gz::sim::System, rgl::RGLServerPluginManager::ISystemConfigure, rgl::RGLServerPluginManager::ISystemPostUpdate ) @@ -30,10 +31,10 @@ namespace rgl { void RGLServerPluginManager::Configure( - const ignition::gazebo::Entity& entity, + const gz::sim::Entity& entity, const std::shared_ptr&, - ignition::gazebo::EntityComponentManager& ecm, - ignition::gazebo::EventManager& evm) + gz::sim::EntityComponentManager& ecm, + gz::sim::EventManager& evm) { ValidateRGLVersion(); if (!CheckRGL(rgl_configure_logging(RGL_LOG_LEVEL_ERROR, nullptr, true))) { @@ -42,19 +43,19 @@ void RGLServerPluginManager::Configure( } void RGLServerPluginManager::PostUpdate( - const ignition::gazebo::UpdateInfo& info, - const ignition::gazebo::EntityComponentManager& ecm) + const gz::sim::UpdateInfo& info, + const gz::sim::EntityComponentManager& ecm) { - ecm.EachNew<>([&](const ignition::gazebo::Entity& entity)-> bool { + ecm.EachNew<>([&](const gz::sim::Entity& entity)-> bool { return RegisterNewLidarCb(entity, ecm);}); - ecm.EachNew + ecm.EachNew (std::bind(&RGLServerPluginManager::LoadEntityToRGLCb, this, _1, _2, _3)); - ecm.EachRemoved<>([&](const ignition::gazebo::Entity& entity)-> bool { + ecm.EachRemoved<>([&](const gz::sim::Entity& entity)-> bool { return UnregisterLidarCb(entity, ecm);}); - ecm.EachRemoved + ecm.EachRemoved (std::bind(&RGLServerPluginManager::RemoveEntityFromRGLCb, this, _1, _2, _3)); UpdateRGLEntityPoses(ecm); diff --git a/RGLServerPlugin/src/Scene.cc b/RGLServerPlugin/src/Scene.cc index 9683786..fb97770 100644 --- a/RGLServerPlugin/src/Scene.cc +++ b/RGLServerPlugin/src/Scene.cc @@ -12,9 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include -#include +#include +#include +#include #include "RGLServerPluginManager.hh" @@ -28,17 +28,17 @@ namespace rgl // always returns true, because the ecm will stop if it encounters false bool RGLServerPluginManager::RegisterNewLidarCb( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm) + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm) { // Plugin must be inside CustomSensor - if (!ecm.EntityHasComponentType(entity, ignition::gazebo::components::CustomSensor::typeId)) + if (!ecm.EntityHasComponentType(entity, gz::sim::components::CustomSensor::typeId)) { return true; } // Looking for plugin - auto pluginData = ecm.ComponentData(entity); + auto pluginData = ecm.ComponentData(entity); if (pluginData == std::nullopt) { return true; } @@ -68,8 +68,8 @@ bool RGLServerPluginManager::RegisterNewLidarCb( // always returns true, because the ecm will stop if it encounters false bool RGLServerPluginManager::UnregisterLidarCb( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm) + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm) { if (!lidarEntities.contains(entity)) { return true; @@ -83,9 +83,9 @@ bool RGLServerPluginManager::UnregisterLidarCb( // always returns true, because the ecm will stop if it encounters false bool RGLServerPluginManager::LoadEntityToRGLCb( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::components::Visual*, - const ignition::gazebo::components::Geometry* geometry) + const gz::sim::Entity& entity, + const gz::sim::components::Visual*, + const gz::sim::components::Geometry* geometry) { if (entitiesToIgnore.contains(entity)) { return true; @@ -110,9 +110,9 @@ bool RGLServerPluginManager::LoadEntityToRGLCb( // always returns true, because the ecm will stop if it encounters false bool RGLServerPluginManager::RemoveEntityFromRGLCb( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::components::Visual*, - const ignition::gazebo::components::Geometry*) + const gz::sim::Entity& entity, + const gz::sim::components::Visual*, + const gz::sim::components::Geometry*) { if (entitiesToIgnore.contains(entity)) { entitiesToIgnore.erase(entity); @@ -132,7 +132,7 @@ bool RGLServerPluginManager::RemoveEntityFromRGLCb( } #pragma clang diagnostic pop -void RGLServerPluginManager::UpdateRGLEntityPoses(const ignition::gazebo::EntityComponentManager& ecm) +void RGLServerPluginManager::UpdateRGLEntityPoses(const gz::sim::EntityComponentManager& ecm) { for (auto entity: entitiesInRgl) { rgl_mat3x4f rglMatrix = FindWorldPoseInRglMatrix(entity.first, ecm); @@ -142,13 +142,13 @@ void RGLServerPluginManager::UpdateRGLEntityPoses(const ignition::gazebo::Entity } } -std::unordered_set RGLServerPluginManager::GetEntitiesInParentLink( - ignition::gazebo::Entity entity, - const ignition::gazebo::EntityComponentManager& ecm) +std::unordered_set RGLServerPluginManager::GetEntitiesInParentLink( + gz::sim::Entity entity, + const gz::sim::EntityComponentManager& ecm) { auto parentEntity = ecm.ParentEntity(entity); - if (parentEntity == ignition::gazebo::kNullEntity || - !ecm.EntityHasComponentType(parentEntity, ignition::gazebo::components::Link::typeId)) { + if (parentEntity == gz::sim::kNullEntity || + !ecm.EntityHasComponentType(parentEntity, gz::sim::components::Link::typeId)) { return {}; } return ecm.Descendants(parentEntity); diff --git a/RGLServerPlugin/src/Utils.cc b/RGLServerPlugin/src/Utils.cc index 774612c..032728d 100644 --- a/RGLServerPlugin/src/Utils.cc +++ b/RGLServerPlugin/src/Utils.cc @@ -15,6 +15,7 @@ #include #include "Utils.hh" +#include "gz/math/Matrix4.hh" #define WORLD_ENTITY_ID 1 @@ -33,25 +34,25 @@ bool CheckRGL(rgl_status_t status) return false; } -ignition::math::Pose3 FindWorldPose( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::EntityComponentManager& ecm) +gz::math::Pose3 FindWorldPose( + const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm) { - auto localPose = ecm.Component(entity); + auto localPose = ecm.Component(entity); if (nullptr == localPose) { ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; - return ignition::math::Pose3d::Zero; + return gz::math::Pose3d::Zero; } auto worldPose = localPose->Data(); - ignition::gazebo::Entity thisEntity = entity; - ignition::gazebo::Entity parent; + gz::sim::Entity thisEntity = entity; + gz::sim::Entity parent; while ((parent = ecm.ParentEntity(thisEntity)) != WORLD_ENTITY_ID) { - auto parentPose = ecm.Component(parent); + auto parentPose = ecm.Component(parent); if (nullptr == parentPose) { ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; - return ignition::math::Pose3d::Zero; + return gz::math::Pose3d::Zero; } worldPose += parentPose->Data(); thisEntity = parent; @@ -61,17 +62,17 @@ ignition::math::Pose3 FindWorldPose( } rgl_mat3x4f FindWorldPoseInRglMatrix( - const ignition::gazebo::Entity& entity, - const ignition::gazebo::EntityComponentManager& ecm) + const gz::sim::Entity& entity, + const gz::sim::EntityComponentManager& ecm) { return IgnPose3dToRglMatrix(FindWorldPose(entity, ecm)); } rgl_mat3x4f IgnPose3dToRglMatrix( - const ignition::math::Pose3& pose) + const gz::math::Pose3& pose) { - auto ignMatrix = ignition::math::Matrix4(pose); + auto ignMatrix = gz::math::Matrix4(pose); rgl_mat3x4f rglMatrix; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 4; ++j) { diff --git a/RGLVisualize/RGLVisualize.cc b/RGLVisualize/RGLVisualize.cc index f9c7998..ed4a50b 100644 --- a/RGLVisualize/RGLVisualize.cc +++ b/RGLVisualize/RGLVisualize.cc @@ -20,15 +20,16 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include -#include -#include +#include +#include #include "RGLVisualize.hh" @@ -45,7 +46,7 @@ class RGLVisualizePrivate public: void ClearMarkers(); /// \brief Transport node - public: ignition::transport::Node node; + public: gz::transport::Node node; /// \brief Name of topic for PointCloudPacked public: std::string pointCloudTopic; @@ -57,7 +58,7 @@ class RGLVisualizePrivate public: std::recursive_mutex mutex; /// \brief Point cloud message containing XYZ positions - public: ignition::msgs::PointCloudPacked pointCloudMsg; + public: gz::msgs::PointCloudPacked pointCloudMsg; /// \brief True if showing, changeable at runtime public: bool showing{true}; @@ -65,7 +66,7 @@ class RGLVisualizePrivate ///////////////////////////////////////////////// RGLVisualize::RGLVisualize() - : ignition::gui::Plugin(), + : gz::gui::Plugin(), dataPtr(std::make_unique()) { this->OnRefresh(); @@ -98,7 +99,7 @@ void RGLVisualize::LoadConfig(const tinyxml2::XMLElement *_pluginElem) } } - ignition::gui::App()->findChild()->installEventFilter(this); + gz::gui::App()->findChild()->installEventFilter(this); } ////////////////////////////////////////////////// @@ -161,11 +162,11 @@ void RGLVisualize::OnRefresh() this->dataPtr->node.TopicList(allTopics); for (const auto& topic : allTopics) { - std::vector publishers; + std::vector publishers; this->dataPtr->node.TopicInfo(topic, publishers); for (const auto& pub : publishers) { - if (pub.MsgTypeName() == "ignition.msgs.PointCloudPacked") + if (pub.MsgTypeName() == "gz.msgs.PointCloudPacked") { if (std::find(this->dataPtr->pointCloudTopicList.begin(), this->dataPtr->pointCloudTopicList.end(), @@ -203,7 +204,7 @@ void RGLVisualize::SetPointCloudTopicList( ////////////////////////////////////////////////// void RGLVisualize::OnPointCloud( - const ignition::msgs::PointCloudPacked &_msg) + const gz::msgs::PointCloudPacked &_msg) { std::lock_guard lock(this->dataPtr->mutex); this->dataPtr->pointCloudMsg = _msg; @@ -212,7 +213,7 @@ void RGLVisualize::OnPointCloud( ////////////////////////////////////////////////// void RGLVisualize::OnPointCloudService( - const ignition::msgs::PointCloudPacked &_msg, bool _result) + const gz::msgs::PointCloudPacked &_msg, bool _result) { if (!_result) { @@ -225,7 +226,7 @@ void RGLVisualize::OnPointCloudService( ////////////////////////////////////////////////// void RGLVisualizePrivate::PublishMarkers() { - IGN_PROFILE("RGLVisualize::PublishMarkers"); + GZ_PROFILE("RGLVisualize::PublishMarkers"); if (!this->showing) { @@ -240,18 +241,18 @@ void RGLVisualizePrivate::PublishMarkers() } std::lock_guard lock(this->mutex); - ignition::msgs::Marker marker; + gz::msgs::Marker marker; marker.set_ns(this->pointCloudTopic); marker.set_id(1); - marker.set_action(ignition::msgs::Marker::ADD_MODIFY); - marker.set_type(ignition::msgs::Marker::POINTS); - marker.set_visibility(ignition::msgs::Marker::GUI); + marker.set_action(gz::msgs::Marker::ADD_MODIFY); + marker.set_type(gz::msgs::Marker::POINTS); + marker.set_visibility(gz::msgs::Marker::GUI); - ignition::msgs::PointCloudPackedIterator + gz::msgs::PointCloudPackedIterator iterX(this->pointCloudMsg, "x"); - ignition::msgs::PointCloudPackedIterator + gz::msgs::PointCloudPackedIterator iterY(this->pointCloudMsg, "y"); - ignition::msgs::PointCloudPackedIterator + gz::msgs::PointCloudPackedIterator iterZ(this->pointCloudMsg, "z"); // Index of point in point cloud, visualized or not @@ -266,8 +267,8 @@ void RGLVisualizePrivate::PublishMarkers() for (; ptIdx < std::min((int)this->pointCloudMsg.data().size(), (int)num_points); ++iterX, ++iterY, ++iterZ, ++ptIdx) { - ignition::msgs::Set(marker.add_point(), - ignition::math::Vector3d(*iterX, + gz::msgs::Set(marker.add_point(), + gz::math::Vector3d(*iterX, *iterY, *iterZ)); } @@ -284,10 +285,10 @@ void RGLVisualizePrivate::ClearMarkers() } std::lock_guard lock(this->mutex); - ignition::msgs::Marker msg; + gz::msgs::Marker msg; msg.set_ns(this->pointCloudTopic); msg.set_id(0); - msg.set_action(ignition::msgs::Marker::DELETE_ALL); + msg.set_action(gz::msgs::Marker::DELETE_ALL); igndbg << "Clearing markers on " << this->pointCloudTopic @@ -297,6 +298,6 @@ void RGLVisualizePrivate::ClearMarkers() } // Register this plugin -IGNITION_ADD_PLUGIN(RGLVisualize, ::ignition::gui::Plugin) +GZ_ADD_PLUGIN(RGLVisualize, ::gz::gui::Plugin) } // namespace rgl diff --git a/RGLVisualize/RGLVisualize.hh b/RGLVisualize/RGLVisualize.hh index d45e005..840fb37 100644 --- a/RGLVisualize/RGLVisualize.hh +++ b/RGLVisualize/RGLVisualize.hh @@ -21,15 +21,15 @@ #include -#include -#include +#include +#include namespace rgl { class RGLVisualizePrivate; -/// \brief Visualize `ignition::msgs::PointCloudPacked` messages in a 3D +/// \brief Visualize `gz::msgs::PointCloudPacked` messages in a 3D /// scene. /// /// This is a port of PointCloud gui plugin from Gazebo Garden to enable non-uniform @@ -40,8 +40,8 @@ class RGLVisualizePrivate; /// /// Parameters: /// * ``: Topic to receive -/// `ignition::msgs::PointCloudPacked` messages. -class RGLVisualize : public ignition::gui::Plugin +/// `gz::msgs::PointCloudPacked` messages. +class RGLVisualize : public gz::gui::Plugin { Q_OBJECT @@ -64,12 +64,12 @@ class RGLVisualize : public ignition::gui::Plugin /// \brief Callback function for point cloud topic. /// \param[in] _msg Point cloud message - public: void OnPointCloud(const ignition::msgs::PointCloudPacked &_msg); + public: void OnPointCloud(const gz::msgs::PointCloudPacked &_msg); /// \brief Callback function for point cloud service /// \param[in] _msg Point cloud message /// \param[out] _result True on success. - public: void OnPointCloudService(const ignition::msgs::PointCloudPacked &_msg, + public: void OnPointCloudService(const gz::msgs::PointCloudPacked &_msg, bool _result); /// \brief Get the topic list diff --git a/RGLVisualize/RGLVisualize.qml b/RGLVisualize/RGLVisualize.qml index ebb0ff3..1a9d11b 100644 --- a/RGLVisualize/RGLVisualize.qml +++ b/RGLVisualize/RGLVisualize.qml @@ -21,7 +21,7 @@ import QtQuick.Controls 2.1 import QtQuick.Dialogs 1.0 import QtQuick.Controls.Material 2.1 import QtQuick.Layouts 1.3 -import ignition.gui 1.0 +import gz.gui 1.0 import "qrc:/qml" ColumnLayout { From 76622717e845ffb8de45e1809a49ff587a6d9457 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 29 May 2024 18:58:34 +0200 Subject: [PATCH 03/10] Update test worlds --- test_world/rgl_playground.sdf | 8 ++++---- test_world/sonoma_with_rgl.sdf | 12 ++++++------ 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/test_world/rgl_playground.sdf b/test_world/rgl_playground.sdf index 75c476f..4b02482 100644 --- a/test_world/rgl_playground.sdf +++ b/test_world/rgl_playground.sdf @@ -9,10 +9,10 @@ 1 1000 - - - - + + + + 0 0 -9.8 6e-06 2.3e-05 -4.2e-05 diff --git a/test_world/sonoma_with_rgl.sdf b/test_world/sonoma_with_rgl.sdf index 37a3835..ae921d8 100644 --- a/test_world/sonoma_with_rgl.sdf +++ b/test_world/sonoma_with_rgl.sdf @@ -10,12 +10,12 @@ 0.7 0.7 0.7 1 true - - + + ogre2 - - + + @@ -76,7 +76,7 @@ /cmd_vel - + rgl_lidar/world @@ -1068,7 +1068,7 @@ 0 0 0 0 -0 0 - /cmd_vel front_left_wheel_joint From 009d693587c0393ebe876a82d46a234e1c2279bf Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 29 May 2024 19:27:06 +0200 Subject: [PATCH 04/10] Aling readme to gz harmonic --- README.md | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/README.md b/README.md index 2b028e6..2ee2a0c 100644 --- a/README.md +++ b/README.md @@ -14,7 +14,7 @@ RGL Gazebo Plugin has been created by [Robotec.AI](https://robotec.ai/) to bring Key features: - Point cloud computation using hardware-accelerated raytracing (Nvidia OptiX) - High performance (~4x improvement over `gpu_lidar` sensor from Gazebo) -- Multiple LiDAR pattern configuration methods, including importing a pattern from a binary file +- Multiple LiDAR pattern configuration methods, including importing a pattern from a binary file - Realistic presets of the most popular LiDARs ## Requirements: @@ -46,8 +46,8 @@ Key features: # libRGLServerPluginInstance.so, libRGLServerPluginManager.so and libRobotecGPULidar.so # are located in RGLServerPlugin directory, # and libRGLVisualize.so in RGLGuiPlugin. - export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/RGLServerPlugin:$IGN_GAZEBO_SYSTEM_PLUGIN_PATH - export IGN_GUI_PLUGIN_PATH=`pwd`/RGLGuiPlugin:$IGN_GUI_PLUGIN_PATH + export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/RGLServerPlugin:$GZ_SIM_SYSTEM_PLUGIN_PATH + export GZ_GUI_PLUGIN_PATH=`pwd`/RGLGuiPlugin:$GZ_GUI_PLUGIN_PATH ``` ### Building from source ```shell @@ -57,8 +57,8 @@ cmake .. make -j make install cd .. -export IGN_GAZEBO_SYSTEM_PLUGIN_PATH=`pwd`/install/RGLServerPlugin:$IGN_GAZEBO_SYSTEM_PLUGIN_PATH -export IGN_GUI_PLUGIN_PATH=`pwd`/install/RGLVisualize:$IGN_GUI_PLUGIN_PATH +export GZ_SIM_SYSTEM_PLUGIN_PATH=`pwd`/install/RGLServerPlugin:$GZ_SIM_SYSTEM_PLUGIN_PATH +export GZ_GUI_PLUGIN_PATH=`pwd`/install/RGLVisualize:$GZ_GUI_PLUGIN_PATH ``` ## Demo: @@ -66,7 +66,7 @@ export IGN_GUI_PLUGIN_PATH=`pwd`/install/RGLVisualize:$IGN_GUI_PLUGIN_PATH Launch the prepared simulation from `test_world` directory: ```shell -ign gazebo sonoma_with_rgl.sdf +gz sim sonoma_with_rgl.sdf ``` 1. Start the simulation by pressing play @@ -77,7 +77,7 @@ The second sample world (rgl_playground.sdf) contains all supported object types ```shell # From the top-level directory of this repository export RGL_PATTERNS_DIR=`pwd`/lidar_patterns -ign gazebo test_world/rgl_playground.sdf +gz sim test_world/rgl_playground.sdf ``` ## Using the plugin: @@ -113,7 +113,7 @@ Inside the link entity in your model, add a custom sensor: - **update_rate** - the frequency at which the lidar will perform raycasting (in Hz). -- **topic** - topic on which pointcloud message (ignition::msgs::PointCloudPacked) will be published. A second topic with the `/world` postfix will also be created for visualization purposes. +- **topic** - topic on which pointcloud message (gz::msgs::PointCloudPacked) will be published. A second topic with the `/world` postfix will also be created for visualization purposes. - **frame** - frame_id for point cloud message header. @@ -153,7 +153,7 @@ Inside the link entity in your model, add a custom sensor: ``` - **pattern_preset**\ - We have prepared several lidar presets. You can type in the name of a LiDAR to use its pattern (all available patterns are shown below). + We have prepared several lidar presets. You can type in the name of a LiDAR to use its pattern (all available patterns are shown below). ```xml Alpha Prime Puck @@ -186,8 +186,8 @@ Inside the link entity in your model, add a custom sensor: ``` - **pattern_lidar2d**\ - Almost the same as `pattern_uniform` but only has - the `horizontal` element and publishes a + Almost the same as `pattern_uniform` but only has + the `horizontal` element and publishes a `LaserScan` message instead of a point cloud ```xml From 5dab890ec9fd7a4633bc5924afafba6911d165ab Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Wed, 29 May 2024 19:30:05 +0200 Subject: [PATCH 05/10] Aling readme to gz harmonic, fix link to gz --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 2ee2a0c..0c4f950 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ Key features: - OS: [Ubuntu 20.04](https://releases.ubuntu.com/focal/) or [Ubuntu 22.04](https://releases.ubuntu.com/jammy/) -- Gazebo: [Fortress 6.14](https://gazebosim.org/docs/fortress/install) +- Gazebo: [Fortress 8.3](https://gazebosim.org/docs/harmonic/installl) - GPU: CUDA-enabled From a31e2da1d95ca64a1a4e38f33b4f72b1693d82a6 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Thu, 30 May 2024 11:30:33 +0200 Subject: [PATCH 06/10] Fix warnings about deprecated functions calls. --- RGLServerPlugin/src/Utils.cc | 2 +- RGLVisualize/RGLVisualize.cc | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/RGLServerPlugin/src/Utils.cc b/RGLServerPlugin/src/Utils.cc index 032728d..9c23e0a 100644 --- a/RGLServerPlugin/src/Utils.cc +++ b/RGLServerPlugin/src/Utils.cc @@ -54,7 +54,7 @@ gz::math::Pose3 FindWorldPose( ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; return gz::math::Pose3d::Zero; } - worldPose += parentPose->Data(); + worldPose = parentPose->Data() * worldPose; thisEntity = parent; } diff --git a/RGLVisualize/RGLVisualize.cc b/RGLVisualize/RGLVisualize.cc index ed4a50b..b51bced 100644 --- a/RGLVisualize/RGLVisualize.cc +++ b/RGLVisualize/RGLVisualize.cc @@ -163,7 +163,8 @@ void RGLVisualize::OnRefresh() for (const auto& topic : allTopics) { std::vector publishers; - this->dataPtr->node.TopicInfo(topic, publishers); + std::vector subscriber; + this->dataPtr->node.TopicInfo(topic, publishers, subscriber); for (const auto& pub : publishers) { if (pub.MsgTypeName() == "gz.msgs.PointCloudPacked") From 0c7ef0d896274d2691fa605e46bea2dcea4de0ba Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 3 Jan 2025 17:56:26 +0000 Subject: [PATCH 07/10] Migrate to Ionic --- RGLServerPlugin/CMakeLists.txt | 10 ++--- RGLServerPlugin/src/Lidar.cc | 34 ++++++++-------- RGLServerPlugin/src/LidarPatternLoader.cc | 40 +++++++++---------- RGLServerPlugin/src/Mesh.cc | 6 +-- RGLServerPlugin/src/RGLServerPluginManager.cc | 2 +- RGLServerPlugin/src/Scene.cc | 12 +++--- RGLServerPlugin/src/Utils.cc | 6 +-- RGLVisualize/CMakeLists.txt | 14 +++---- RGLVisualize/RGLVisualize.cc | 14 +++---- 9 files changed, 69 insertions(+), 69 deletions(-) diff --git a/RGLServerPlugin/CMakeLists.txt b/RGLServerPlugin/CMakeLists.txt index 642d1e8..14f59b2 100644 --- a/RGLServerPlugin/CMakeLists.txt +++ b/RGLServerPlugin/CMakeLists.txt @@ -2,12 +2,12 @@ cmake_minimum_required(VERSION 3.16) project(RGLServerPlugin) set(CMAKE_CXX_STANDARD 20) -find_package(gz-cmake3 REQUIRED) -gz_find_package(gz-plugin2 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) +find_package(gz-cmake4 REQUIRED) +gz_find_package(gz-plugin3 REQUIRED COMPONENTS register) +set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR}) -gz_find_package(gz-sim8 REQUIRED) -set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) +gz_find_package(gz-sim9 REQUIRED) +set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR}) include_directories(include) diff --git a/RGLServerPlugin/src/Lidar.cc b/RGLServerPlugin/src/Lidar.cc index ec14482..e48875b 100644 --- a/RGLServerPlugin/src/Lidar.cc +++ b/RGLServerPlugin/src/Lidar.cc @@ -30,28 +30,28 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptrHasElement(PARAM_UPDATE_RATE_ID)) { - ignerr << "No '" << PARAM_UPDATE_RATE_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; + gzerr << "No '" << PARAM_UPDATE_RATE_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; return false; } if (!sdf->HasElement(PARAM_RANGE_ID)) { - ignerr << "No '" << PARAM_RANGE_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; + gzerr << "No '" << PARAM_RANGE_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; return false; } if (!sdf->HasElement(PARAM_TOPIC_ID)) { - ignerr << "No '" << PARAM_TOPIC_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; + gzerr << "No '" << PARAM_TOPIC_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; return false; } if (!sdf->HasElement(PARAM_FRAME_ID)) { - ignerr << "No '" << PARAM_FRAME_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; + gzerr << "No '" << PARAM_FRAME_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; return false; } // Optional parameters if (!sdf->HasElement(PARAM_UPDATE_ON_PAUSED_SIM_ID)) { - ignwarn << "No '" << PARAM_UPDATE_ON_PAUSED_SIM_ID << "' parameter specified for the RGL lidar. " + gzwarn << "No '" << PARAM_UPDATE_ON_PAUSED_SIM_ID << "' parameter specified for the RGL lidar. " << "Using default value: " << updateOnPausedSim << "\n"; } else { updateOnPausedSim = sdf->Get(PARAM_UPDATE_ON_PAUSED_SIM_ID); @@ -70,7 +70,7 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptrHasElement("pattern_lidar2d")) { - ignmsg << "Lidar is 2D, switching to publish LaserScan messages"; + gzmsg << "Lidar is 2D, switching to publish LaserScan messages"; publishLaserScan = true; resultDistances.resize(lidarPattern.size()); scanHMin = sdf->FindElement("pattern_lidar2d")->FindElement("horizontal")->Get("min_angle"); @@ -110,7 +110,7 @@ void RGLServerPluginInstance::CreateLidar(gz::sim::Entity entity, !CheckRGL(rgl_node_points_yield(&rglNodeYield, yieldFields.data(), yieldFields.size())) || !CheckRGL(rgl_node_points_transform(&rglNodeToLidarFrame, &identity))) { - ignerr << "Failed to create RGL nodes when initializing lidar. Disabling plugin.\n"; + gzerr << "Failed to create RGL nodes when initializing lidar. Disabling plugin.\n"; return; } @@ -119,24 +119,24 @@ void RGLServerPluginInstance::CreateLidar(gz::sim::Entity entity, !CheckRGL(rgl_graph_node_add_child(rglNodeRaytrace, rglNodeCompact)) || !CheckRGL(rgl_graph_node_add_child(rglNodeCompact, rglNodeToLidarFrame))) { - ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; + gzerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; return; } if (!publishLaserScan) { if(!CheckRGL(rgl_graph_node_add_child(rglNodeToLidarFrame, rglNodeYield))) { - ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; + gzerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; } - ignmsg << "Start publishing PointCloudPacked messages on topic '" << topicName << "'\n"; + gzmsg << "Start publishing PointCloudPacked messages on topic '" << topicName << "'\n"; pointCloudPublisher = gazeboNode.Advertise(topicName); } else { if(!CheckRGL(rgl_graph_node_add_child(rglNodeRaytrace, rglNodeYield))) { - ignerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; + gzerr << "Failed to connect RGL nodes when initializing lidar. Disabling plugin.\n"; } - ignmsg << "Start publishing LaserScan messages on topic '" << topicName << "'\n"; + gzmsg << "Start publishing LaserScan messages on topic '" << topicName << "'\n"; laserScanPublisher = gazeboNode.Advertise(topicName); } @@ -186,7 +186,7 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi lastRaytraceTime = simTime; if (!CheckRGL(rgl_graph_run(rglNodeRaytrace))) { - ignerr << "Failed to perform raytrace.\n"; + gzerr << "Failed to perform raytrace.\n"; return; } @@ -196,14 +196,14 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi if (!CheckRGL(rgl_graph_get_result_size(rglNodeYield, RGL_FIELD_XYZ_F32, &hitpointCount, nullptr)) || !CheckRGL(rgl_graph_get_result_data(rglNodeYield, RGL_FIELD_XYZ_F32, resultPointCloud.data()))) { - ignerr << "Failed to get result data from RGL lidar.\n"; + gzerr << "Failed to get result data from RGL lidar.\n"; return; } } else { if (!CheckRGL(rgl_graph_get_result_size(rglNodeYield, RGL_FIELD_DISTANCE_F32, &hitpointCount, nullptr)) || !CheckRGL(rgl_graph_get_result_data(rglNodeYield, RGL_FIELD_DISTANCE_F32, resultDistances.data()))) { - ignerr << "Failed to get result distances from RGL lidar.\n"; + gzerr << "Failed to get result distances from RGL lidar.\n"; return; } } @@ -220,7 +220,7 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi if (!CheckRGL(rgl_graph_get_result_size(rglNodeToLidarFrame, RGL_FIELD_XYZ_F32, &hitpointCount, nullptr)) || !CheckRGL(rgl_graph_get_result_data(rglNodeCompact, RGL_FIELD_XYZ_F32, resultPointCloud.data()))) { - ignerr << "Failed to get visualization data from RGL lidar.\n"; + gzerr << "Failed to get visualization data from RGL lidar.\n"; return; } auto worldMsg = CreatePointCloudMsg(simTime, worldFrameId, hitpointCount); @@ -280,7 +280,7 @@ void RGLServerPluginInstance::DestroyLidar() } if (!CheckRGL(rgl_graph_destroy(rglNodeRaytrace))) { - ignerr << "Failed to destroy RGL lidar.\n"; + gzerr << "Failed to destroy RGL lidar.\n"; } // Reset publishers if (!publishLaserScan) { diff --git a/RGLServerPlugin/src/LidarPatternLoader.cc b/RGLServerPlugin/src/LidarPatternLoader.cc index debe4cc..aac21d8 100644 --- a/RGLServerPlugin/src/LidarPatternLoader.cc +++ b/RGLServerPlugin/src/LidarPatternLoader.cc @@ -49,13 +49,13 @@ bool LidarPatternLoader::Load(const sdf::ElementConstPtr& sdf, std::vectorHasElement(patterName)) { continue; } - ignmsg << "Trying to load '" << patterName << "' pattern...\n"; + gzmsg << "Trying to load '" << patterName << "' pattern...\n"; if (loadFunction(sdf->FindElement(patterName), outPattern)) { - ignmsg << "Successfully loaded pattern '" << patterName << "'.\n"; + gzmsg << "Successfully loaded pattern '" << patterName << "'.\n"; return true; } } - ignerr << "Failed to load lidar pattern. See plugin's documentation for available options.\n"; + gzerr << "Failed to load lidar pattern. See plugin's documentation for available options.\n"; return false; } @@ -64,17 +64,17 @@ bool LidarPatternLoader::LoadAnglesAndSamplesElement(const sdf::ElementConstPtr& int& samples) { if (!sdf->HasElement("samples")) { - ignerr << "Failed to load '" << sdf->GetName() << "' element. A 'samples' element is required inside, but it is not set.\n"; + gzerr << "Failed to load '" << sdf->GetName() << "' element. A 'samples' element is required inside, but it is not set.\n"; return false; } if (!sdf->HasElement("min_angle")) { - ignerr << "Failed to load '" << sdf->GetName() << "' element. A 'min_angle' element is required inside, but it is not set.\n"; + gzerr << "Failed to load '" << sdf->GetName() << "' element. A 'min_angle' element is required inside, but it is not set.\n"; return false; } if (!sdf->HasElement("max_angle")) { - ignerr << "Failed to load '" << sdf->GetName() << "' element. A 'max_angle' element is required inside, but it is not set.\n"; + gzerr << "Failed to load '" << sdf->GetName() << "' element. A 'max_angle' element is required inside, but it is not set.\n"; return false; } @@ -83,12 +83,12 @@ bool LidarPatternLoader::LoadAnglesAndSamplesElement(const sdf::ElementConstPtr& samples = sdf->Get("samples"); if (angleMin > angleMax) { - ignerr << "Failed to load '" << sdf->GetName() << "' element. Min angle greater than vertical max angle.\n"; + gzerr << "Failed to load '" << sdf->GetName() << "' element. Min angle greater than vertical max angle.\n"; return false; } if (samples <= 0) { - ignerr << "Failed to load '" << sdf->GetName() << "' element. Samples must be a positive value.\n"; + gzerr << "Failed to load '" << sdf->GetName() << "' element. Samples must be a positive value.\n"; return false; } @@ -98,12 +98,12 @@ bool LidarPatternLoader::LoadAnglesAndSamplesElement(const sdf::ElementConstPtr& bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, std::vector& outPattern) { if (!sdf->HasElement("vertical")) { - ignerr << "Failed to load uniform pattern. A vertical element is required, but it is not set.\n"; + gzerr << "Failed to load uniform pattern. A vertical element is required, but it is not set.\n"; return false; } if (!sdf->HasElement("horizontal")) { - ignerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n"; + gzerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n"; return false; } @@ -142,7 +142,7 @@ bool LidarPatternLoader::LoadPatternFromUniform(const sdf::ElementConstPtr& sdf, bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::vector& outPattern) { if (!sdf->HasAttribute("channels")) { - ignerr << "Failed to load custom pattern. A channels attribute is required, but it is not set.\n"; + gzerr << "Failed to load custom pattern. A channels attribute is required, but it is not set.\n"; return false; } @@ -155,7 +155,7 @@ bool LidarPatternLoader::LoadPatternFromCustom(const sdf::ElementConstPtr& sdf, std::back_inserter(channels)); if (channels.empty()) { - ignerr << "Failed to load custom pattern. No channels provided.\n"; + gzerr << "Failed to load custom pattern. No channels provided.\n"; return false; } @@ -187,17 +187,17 @@ bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, { auto presetName = sdf->Get(); if (!presetNameToFilename.contains(presetName)) { - ignerr << "Failed to load preset pattern. Preset '" << presetName << "' is not available.\n"; + gzerr << "Failed to load preset pattern. Preset '" << presetName << "' is not available.\n"; return false; } fs::path presetPath = presetNameToFilename[presetName]; if (const char* presetDir = std::getenv(PATTERNS_DIR_ENV)) { presetPath = fs::path(presetDir) / presetNameToFilename[presetName]; } - ignmsg << "Loading pattern_preset '" << presetName << "'...\n"; + gzmsg << "Loading pattern_preset '" << presetName << "'...\n"; outPattern = LoadVector(presetPath); if (outPattern.size() == 0) { - ignerr << "Failed to load preset. Make sure the environment variable '" << PATTERNS_DIR_ENV << "' is set correctly.\n"; + gzerr << "Failed to load preset. Make sure the environment variable '" << PATTERNS_DIR_ENV << "' is set correctly.\n"; return false; } return true; @@ -206,10 +206,10 @@ bool LidarPatternLoader::LoadPatternFromPreset(const sdf::ElementConstPtr& sdf, bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& sdf, std::vector& outPattern) { fs::path presetPath = fs::path(sdf->Get()); - ignmsg << "Loading preset from path '" << presetPath << "'...\n"; + gzmsg << "Loading preset from path '" << presetPath << "'...\n"; outPattern = LoadVector(presetPath); if (outPattern.size() == 0) { - ignerr << "Failed to load preset from path.\n"; + gzerr << "Failed to load preset from path.\n"; return false; } return true; @@ -218,7 +218,7 @@ bool LidarPatternLoader::LoadPatternFromPresetPath(const sdf::ElementConstPtr& s bool LidarPatternLoader::LoadPatternFromLidar2d(const sdf::ElementConstPtr& sdf, std::vector& outPattern) { if (!sdf->HasElement("horizontal")) { - ignerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n"; + gzerr << "Failed to load uniform pattern. A horizontal element is required, but it is not set.\n"; return false; } @@ -255,7 +255,7 @@ std::vector LidarPatternLoader::LoadVector(const fs::path& path) std::ifstream file(path, std::ios::binary); if (!file.is_open() || file.eof()) { - ignerr << "failed to open file '" << path << "' or file is empty, data will not be loaded.\n"; + gzerr << "failed to open file '" << path << "' or file is empty, data will not be loaded.\n"; return std::vector(); } @@ -265,7 +265,7 @@ std::vector LidarPatternLoader::LoadVector(const fs::path& path) file.seekg(0, std::ios::beg); if (fileSize % sizeof(T) != 0) { - ignerr << "invalid file size: '" << path << "'.\n"; + gzerr << "invalid file size: '" << path << "'.\n"; return std::vector(); } diff --git a/RGLServerPlugin/src/Mesh.cc b/RGLServerPlugin/src/Mesh.cc index 735448e..1e949e2 100644 --- a/RGLServerPlugin/src/Mesh.cc +++ b/RGLServerPlugin/src/Mesh.cc @@ -125,7 +125,7 @@ RGLServerPluginManager::MeshInfo RGLServerPluginManager::LoadMesh( const gz::common::Mesh* mesh = meshManager->Load(meshPath); if (mesh == nullptr) { - ignerr << "Failed to import mesh to RGL: " << meshPath << ".\n"; + gzerr << "Failed to import mesh to RGL: " << meshPath << ".\n"; return std::monostate{}; } @@ -145,7 +145,7 @@ RGLServerPluginManager::MeshInfo RGLServerPluginManager::LoadMesh( return subMeshCopy; } } - ignerr << "Failed to import subMesh to RGL: " << subMeshName << ".\n"; + gzerr << "Failed to import subMesh to RGL: " << subMeshName << ".\n"; return std::monostate{}; } @@ -215,7 +215,7 @@ bool RGLServerPluginManager::LoadMeshToRGL( auto meshInfo = GetMeshPointer(data, scaleX, scaleY, scaleZ); if (std::holds_alternative(meshInfo)) { - ignerr << "Failed to load mesh of geometry type '" << static_cast(data.Type()) + gzerr << "Failed to load mesh of geometry type '" << static_cast(data.Type()) << "' to RGL. Skipping...\n"; return false; } diff --git a/RGLServerPlugin/src/RGLServerPluginManager.cc b/RGLServerPlugin/src/RGLServerPluginManager.cc index 3507d7d..550398d 100644 --- a/RGLServerPlugin/src/RGLServerPluginManager.cc +++ b/RGLServerPlugin/src/RGLServerPluginManager.cc @@ -38,7 +38,7 @@ void RGLServerPluginManager::Configure( { ValidateRGLVersion(); if (!CheckRGL(rgl_configure_logging(RGL_LOG_LEVEL_ERROR, nullptr, true))) { - ignerr << "Failed to configure RGL logging.\n"; + gzerr << "Failed to configure RGL logging.\n"; } } diff --git a/RGLServerPlugin/src/Scene.cc b/RGLServerPlugin/src/Scene.cc index fb97770..9808a86 100644 --- a/RGLServerPlugin/src/Scene.cc +++ b/RGLServerPlugin/src/Scene.cc @@ -91,17 +91,17 @@ bool RGLServerPluginManager::LoadEntityToRGLCb( return true; } if (entitiesInRgl.contains(entity)) { - ignwarn << "Trying to add same entity (" << entity << ") to rgl multiple times!\n"; + gzwarn << "Trying to add same entity (" << entity << ") to rgl multiple times!\n"; return true; } rgl_mesh_t rglMesh; if (!LoadMeshToRGL(&rglMesh, geometry->Data())) { - ignerr << "Failed to load mesh to RGL from entity (" << entity << "). Skipping...\n"; + gzerr << "Failed to load mesh to RGL from entity (" << entity << "). Skipping...\n"; return true; } rgl_entity_t rglEntity; if (!CheckRGL(rgl_entity_create(&rglEntity, nullptr, rglMesh))) { - ignerr << "Failed to load entity (" << entity << ") to RGL. Skipping...\n"; + gzerr << "Failed to load entity (" << entity << ") to RGL. Skipping...\n"; return true; } entitiesInRgl.insert({entity, {rglEntity, rglMesh}}); @@ -122,10 +122,10 @@ bool RGLServerPluginManager::RemoveEntityFromRGLCb( return true; } if (!CheckRGL(rgl_entity_destroy(entitiesInRgl.at(entity).first))) { - ignerr << "Failed to remove entity (" << entity << ") from RGL.\n"; + gzerr << "Failed to remove entity (" << entity << ") from RGL.\n"; } if (!CheckRGL(rgl_mesh_destroy(entitiesInRgl.at(entity).second))) { - ignerr << "Failed to remove mesh from entity (" << entity << ") in RGL.\n"; + gzerr << "Failed to remove mesh from entity (" << entity << ") in RGL.\n"; } entitiesInRgl.erase(entity); return true; @@ -137,7 +137,7 @@ void RGLServerPluginManager::UpdateRGLEntityPoses(const gz::sim::EntityComponent for (auto entity: entitiesInRgl) { rgl_mat3x4f rglMatrix = FindWorldPoseInRglMatrix(entity.first, ecm); if (!CheckRGL(rgl_entity_set_pose(entity.second.first, &rglMatrix))) { - ignerr << "Failed to update pose for entity (" << entity.first << ").\n"; + gzerr << "Failed to update pose for entity (" << entity.first << ").\n"; } } } diff --git a/RGLServerPlugin/src/Utils.cc b/RGLServerPlugin/src/Utils.cc index 9c23e0a..1eee2c2 100644 --- a/RGLServerPlugin/src/Utils.cc +++ b/RGLServerPlugin/src/Utils.cc @@ -30,7 +30,7 @@ bool CheckRGL(rgl_status_t status) const char* msg; rgl_get_last_error_string(&msg); - ignerr << msg << "\n"; + gzerr << msg << "\n"; return false; } @@ -40,7 +40,7 @@ gz::math::Pose3 FindWorldPose( { auto localPose = ecm.Component(entity); if (nullptr == localPose) { - ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; + gzmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; return gz::math::Pose3d::Zero; } auto worldPose = localPose->Data(); @@ -51,7 +51,7 @@ gz::math::Pose3 FindWorldPose( while ((parent = ecm.ParentEntity(thisEntity)) != WORLD_ENTITY_ID) { auto parentPose = ecm.Component(parent); if (nullptr == parentPose) { - ignmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; + gzmsg << "pose data missing - using default pose (0, 0, 0, 0, 0, 0)\n"; return gz::math::Pose3d::Zero; } worldPose = parentPose->Data() * worldPose; diff --git a/RGLVisualize/CMakeLists.txt b/RGLVisualize/CMakeLists.txt index 0b22237..f5c6411 100644 --- a/RGLVisualize/CMakeLists.txt +++ b/RGLVisualize/CMakeLists.txt @@ -9,16 +9,16 @@ project(RGLVisualize) set(CMAKE_AUTOMOC ON) -find_package(gz-cmake3 REQUIRED) -gz_find_package(gz-msgs10 REQUIRED) -set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) +find_package(gz-cmake4 REQUIRED) +gz_find_package(gz-msgs11 REQUIRED) +set(GZ_MSGS_VER ${gz-msgs11_VERSION_MAJOR}) -gz_find_package(gz-rendering8 REQUIRED) -set(GZ_RENDERING_VER ${gz-rendering8_VERSION_MAJOR}) +gz_find_package(gz-rendering9 REQUIRED) +set(GZ_RENDERING_VER ${gz-rendering9_VERSION_MAJOR}) -gz_find_package(gz-gui8 REQUIRED) -set(GZ_GUI_VER ${gz-gui8_VERSION_MAJOR}) +gz_find_package(gz-gui9 REQUIRED) +set(GZ_GUI_VER ${gz-gui9_VERSION_MAJOR}) QT5_ADD_RESOURCES(resources_RCC RGLVisualize.qrc) diff --git a/RGLVisualize/RGLVisualize.cc b/RGLVisualize/RGLVisualize.cc index b51bced..2dd0244 100644 --- a/RGLVisualize/RGLVisualize.cc +++ b/RGLVisualize/RGLVisualize.cc @@ -110,7 +110,7 @@ void RGLVisualize::OnPointCloudTopic(const QString &_pointCloudTopic) if (!this->dataPtr->pointCloudTopic.empty() && !this->dataPtr->node.Unsubscribe(this->dataPtr->pointCloudTopic)) { - ignerr << "Unable to unsubscribe from topic [" + gzerr << "Unable to unsubscribe from topic [" << this->dataPtr->pointCloudTopic << "]" << std::endl; } @@ -127,11 +127,11 @@ void RGLVisualize::OnPointCloudTopic(const QString &_pointCloudTopic) if (!this->dataPtr->node.Subscribe(this->dataPtr->pointCloudTopic, &RGLVisualize::OnPointCloud, this)) { - ignerr << "Unable to subscribe to topic [" + gzerr << "Unable to subscribe to topic [" << this->dataPtr->pointCloudTopic << "]\n"; return; } - ignmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl; + gzmsg << "Subscribed to " << this->dataPtr->pointCloudTopic << std::endl; } ////////////////////////////////////////////////// @@ -152,7 +152,7 @@ void RGLVisualize::Show(bool _show) void RGLVisualize::OnRefresh() { std::lock_guard lock(this->dataPtr->mutex); - ignmsg << "Refreshing topic list for point cloud messages." << std::endl; + gzmsg << "Refreshing topic list for point cloud messages." << std::endl; // Clear this->dataPtr->pointCloudTopicList.clear(); @@ -218,7 +218,7 @@ void RGLVisualize::OnPointCloudService( { if (!_result) { - ignerr << "Service request failed." << std::endl; + gzerr << "Service request failed." << std::endl; return; } this->OnPointCloud(_msg); @@ -262,7 +262,7 @@ void RGLVisualizePrivate::PublishMarkers() this->pointCloudMsg.data().size() / this->pointCloudMsg.point_step(); if (this->pointCloudMsg.data().size() % this->pointCloudMsg.point_step() != 0) { - ignwarn << "Mal-formatted point cloud" << std::endl; + gzwarn << "Mal-formatted point cloud" << std::endl; } for (; ptIdx < std::min((int)this->pointCloudMsg.data().size(), (int)num_points); @@ -291,7 +291,7 @@ void RGLVisualizePrivate::ClearMarkers() msg.set_id(0); msg.set_action(gz::msgs::Marker::DELETE_ALL); - igndbg << "Clearing markers on " + gzdbg << "Clearing markers on " << this->pointCloudTopic << std::endl; From bedaa213b0d65c8c6c62b7cd9b2faf221713c473 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 3 Jan 2025 18:10:35 +0000 Subject: [PATCH 08/10] Fix --- RGLServerPlugin/include/rgl/api/core.h | 497 ------------------------- RGLServerPlugin/src/Lidar.cc | 24 -- 2 files changed, 521 deletions(-) delete mode 100644 RGLServerPlugin/include/rgl/api/core.h diff --git a/RGLServerPlugin/include/rgl/api/core.h b/RGLServerPlugin/include/rgl/api/core.h deleted file mode 100644 index 545b82c..0000000 --- a/RGLServerPlugin/include/rgl/api/core.h +++ /dev/null @@ -1,497 +0,0 @@ -#pragma once - -#include -#include -#include - -#ifdef __cplusplus -#define NO_MANGLING extern "C" -#else // NOT __cplusplus -#define NO_MANGLING -#endif - -#if defined _WIN32 || defined __CYGWIN__ -#ifdef __GNUC__ - #define RGL_VISIBLE __attribute__ ((dllexport)) - #else - #define RGL_VISIBLE __declspec(dllexport) - #endif // __GNUC__ -#else -#define RGL_VISIBLE __attribute__ ((visibility("default"))) -#if __GNUC__ >= 4 -#define RGL_VISIBLE __attribute__ ((visibility("default"))) -#else -#define RGL_VISIBLE -#endif -#endif // _WIN32 || __CYGWIN__ - -#define RGL_API NO_MANGLING RGL_VISIBLE - -#define RGL_VERSION_MAJOR 0 -#define RGL_VERSION_MINOR 11 -#define RGL_VERSION_PATCH 3 - -/** - * Three consecutive 32-bit floats. - */ -typedef struct -{ - float value[3]; -} rgl_vec3f; - -#ifndef __cplusplus -static_assert(sizeof(rgl_vec3f) == 3 * sizeof(float)); -#endif - -/** - * Three consecutive 32-bit signed integers. - */ -typedef struct -{ - int32_t value[3]; -} rgl_vec3i; - -/** - * Row-major matrix with 3 rows and 4 columns of 32-bit floats. - */ -typedef struct -{ - float value[3][4]; -} rgl_mat3x4f; - -/** - * Represents on-GPU mesh that can be referenced by entities on the scene. - * Each mesh can be referenced by any number of entities on different scenes. - */ -typedef struct Mesh *rgl_mesh_t; - -/** - * Opaque handle representing an object visible to lidars. - * An entity is always bound to exactly one scene. - */ -typedef struct Entity *rgl_entity_t; - -/** - * TODO(prybicki) - */ -typedef struct Node *rgl_node_t; - -/** - * Opaque handle representing a scene - a collection of entities. - * Using scene is optional. NULL can be passed to use an implicit default scene. - */ -typedef struct Scene *rgl_scene_t; - -/** - * Status (error) codes returned by all RGL API functions. - * Unrecoverable errors require reloading the library (restarting the application). - */ -typedef enum -{ - /** - * Operation successful. - * This is a recoverable error :) - */ - RGL_SUCCESS = 0, - - /** - * One of the arguments is invalid (e.g. null pointer) or a number in invalid range. - * Get the error string for more details. - * This is a recoverable error. - */ - RGL_INVALID_ARGUMENT, - - /** - * RGL internal state has been corrupted by a previous unrecoverable error. - * Application must be restarted. - */ - RGL_INVALID_STATE, - - /** - * Indicates that a logging operation (e.g. configuration) was not successful. - * This is an unrecoverable error. - */ - RGL_LOGGING_ERROR, - - /** - * Indicates that provided API object handle is not known by RGL. - * This can be caused by using previously destroyed API object, e.g. - * by previous call to rgl_*_destroy(...) or rgl_cleanup() - * This is a recoverable error. - */ - RGL_INVALID_API_OBJECT, - - /** - * Indicates that a given file could not be opened. - * This is a recoverable error. - */ - RGL_INVALID_FILE_PATH, - - /** - * Indicates that a tape operation was not successful. - * This is a recoverable error. - */ - RGL_TAPE_ERROR, - - /** - * Indicates an error in the pipeline, such as adjacency of incompatible nodes. - * This is a recoverable error. - */ - RGL_INVALID_PIPELINE, - - /** - * Indicates a failure during (lazy) initialization. - * This is an unrecoverable error. - */ - RGL_INITIALIZATION_ERROR, - - /** - * Requested functionality has been not yet implemented. - * This is a recoverable error. - */ - RGL_NOT_IMPLEMENTED = 404, - - /** - * An unhandled internal error has occurred. - * If you see this error, please file a bug report. - * This is an unrecoverable error. - */ - RGL_INTERNAL_EXCEPTION = 500, -} rgl_status_t; - -/** - * Available logging verbosity levels. - */ -typedef enum : int -{ - RGL_LOG_LEVEL_ALL = 0, - RGL_LOG_LEVEL_TRACE = 0, - RGL_LOG_LEVEL_DEBUG = 1, - RGL_LOG_LEVEL_INFO = 2, - RGL_LOG_LEVEL_WARN = 3, - RGL_LOG_LEVEL_ERROR = 4, - RGL_LOG_LEVEL_CRITICAL = 5, - RGL_LOG_LEVEL_OFF = 6, - RGL_LOG_LEVEL_COUNT = 7 -} rgl_log_level_t; - -/** - * Available point attributes, used to specify layout of the binary data. - */ -typedef enum -{ - RGL_FIELD_XYZ_F32 = 1, - RGL_FIELD_INTENSITY_F32, - RGL_FIELD_IS_HIT_I32, - RGL_FIELD_RAY_IDX_U32, - RGL_FIELD_POINT_IDX_U32, - RGL_FIELD_DISTANCE_F32, - RGL_FIELD_AZIMUTH_F32, - RGL_FIELD_RING_ID_U16, - RGL_FIELD_RETURN_TYPE_U8, - RGL_FIELD_TIME_STAMP_F64, - // Dummy fields - RGL_FIELD_PADDING_8 = 1024, - RGL_FIELD_PADDING_16, - RGL_FIELD_PADDING_32, - // Dynamic fields - RGL_FIELD_DYNAMIC_FORMAT = 13842, -} rgl_field_t; - -/******************************** GENERAL ********************************/ - -/** - * Returns data describing semantic version as described in https://semver.org/ - * Version string can be obtained by formatting "{out_major}.{out_minor}.{out_patch}". - * Hash is provided mostly for debugging and issue reporting. - * @param out_major Address to store major version number - * @param out_minor Address to store minor version number - * @param out_patch Address to store patch version number - */ -RGL_API rgl_status_t -rgl_get_version_info(int32_t *out_major, int32_t *out_minor, int32_t *out_patch); - -/** - * Optionally (re)configures internal logging. This feature may be useful for debugging / issue reporting. - * By default (i.e. not calling `rgl_configure_logging`) is equivalent to the following call: - * `rgl_configure_logging(RGL_LOG_LEVEL_INFO, nullptr, true)` - * @param log_level Controls severity of emitted logs: trace=0, debug=1, info=2, warn=3, error=4, critical=5, off=6 - * @param log_file_path Path to the file where logs will be saved. - * The file will be created or truncated on each configuration. - * Pass nullptr to disable logging to file - * @param use_stdout If true, logs will be outputted to stdout. - */ -RGL_API rgl_status_t -rgl_configure_logging(rgl_log_level_t log_level, const char* log_file_path, bool use_stdout); - -/** - * Returns a pointer to a string explaining last error. This function always succeeds. - * Returned pointer is valid only until next RGL API call. - * @param out_error Address to store pointer to string explaining the cause of the given error. - */ -RGL_API void -rgl_get_last_error_string(const char **out_error_string); - -/** - * Removes all user-created API objects: meshes, entities, scenes, lidars, etc. - * Effectively brings the library to the state as-if it was not yet used. - * All API handles are invalidated. - */ -RGL_API rgl_status_t -rgl_cleanup(void); - - -/******************************** MESH ********************************/ - -/** - * Creates mesh from vertex and index arrays. CW/CCW order does not matter. - * Provided arrays are copied to the GPU before this function returns. - * @param out_mesh Address to store the resulting mesh handle - * @param vertices An array of rgl_vec3f or binary-compatible data representing mesh vertices - * @param vertex_count Number of elements in the vertices array - * @param indices An array of rgl_vec3i or binary-compatible data representing mesh indices - * @param index_count Number of elements in the indices array - */ -RGL_API rgl_status_t -rgl_mesh_create(rgl_mesh_t *out_mesh, - const rgl_vec3f *vertices, - int32_t vertex_count, - const rgl_vec3i *indices, - int32_t index_count); - -/** - * Informs that the given mesh will be no longer used. - * The mesh will be destroyed after all referring entities are destroyed. - * @param mesh Mesh to be marked as no longer needed - */ -RGL_API rgl_status_t -rgl_mesh_destroy(rgl_mesh_t mesh); - -/** - * Updates mesh vertex data. The number of vertices must not change. - * This function is intended to update animated meshes. - * @param mesh Mesh to modify - * @param vertices An array of rgl_vec3f or binary-compatible data representing mesh vertices - * @param vertex_count Number of elements in the vertices array. Must be equal to the original vertex count! - */ -RGL_API rgl_status_t -rgl_mesh_update_vertices(rgl_mesh_t mesh, - const rgl_vec3f *vertices, - int32_t vertex_count); - - -/******************************** ENTITY ********************************/ - -/** - * Creates an entity and adds it to the given scene. - * Entity is a lightweight object which pairs a reference to a mesh with a 3D affine transform. - * @param out_entity Handle to the created entity. - * @param scene Scene where entity will be added. Pass NULL to use the default scene. - * @param mesh Handle to the mesh which will represent the entity on the scene. - */ -RGL_API rgl_status_t -rgl_entity_create(rgl_entity_t *out_entity, rgl_scene_t scene, rgl_mesh_t mesh); - -/** - * Removes an entity from the scene and releases its resources (memory). - * This operation does not affect the mesh used by the entity, since it can be shared among other entities. - * @param entity Entity to remove - */ -RGL_API rgl_status_t -rgl_entity_destroy(rgl_entity_t entity); - -/** - * Changes transform (position, rotation, scaling) of the given entity. - * @param entity Entity to modify - * @param transform Pointer to rgl_mat3x4f (or binary-compatible data) representing desired (entity -> world) coordinate system transform. - */ -RGL_API rgl_status_t -rgl_entity_set_pose(rgl_entity_t entity, const rgl_mat3x4f *local_to_world_tf); - -/******************************** NODES ********************************/ - -/** - * Creates or modifies UseRaysMat3x4fNode. - * The node provides initial rays for its children nodes. - * Initial rays are usually provided in device-local coordinate frame, i.e. close to (0, 0, 0). - * Input: none - * Output: rays - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param rays Pointer to 3x4 affine matrices describing rays poses. - * @param ray_count Size of the `rays` array - */ -RGL_API rgl_status_t -rgl_node_rays_from_mat3x4f(rgl_node_t* node, const rgl_mat3x4f* rays, int32_t ray_count); - -/** - * Creates or modifies UseRingIdsNode. - * The node assigns ring ids for existing rays. - * Input: rays - * Output: rays - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param ring_ids Pointer to ring ids. - * @param ray_count Size of the `ring_ids` array. - */ -RGL_API rgl_status_t -rgl_node_rays_set_ring_ids(rgl_node_t* node, const int32_t* ring_ids, int32_t ring_ids_count); - -/** - * Creates or modifies TransformRaysNode. - * Effectively, the node performs the following operation for all rays: `outputRay[i] = (*transform) * inputRay[i]` - * This function can be used to account for the pose of the device. - * Graph input: rays - * Graph output: rays - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param transform Pointer to a single 3x4 affine matrix describing the transformation to be applied. - */ -RGL_API rgl_status_t -rgl_node_rays_transform(rgl_node_t* node, const rgl_mat3x4f* transform); - -// Applies affine transformation, e.g. to change the coordinate frame. -/** - * Creates or modifies TransformPointsNode. - * The node applies affine transformation to all points. - * It can be used to e.g. change coordinate frame after raytracing. - * Note: affects only RGL_FIELD_XYZ_F32. Other fields are not modified. - * Graph input: point cloud - * Graph output: point cloud (transformed) - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param transform Pointer to a single 3x4 affine matrix describing the transformation to be applied. - */ -RGL_API rgl_status_t -rgl_node_points_transform(rgl_node_t* node, const rgl_mat3x4f* transform); - -/** - * Creates or modifies RaytraceNode. - * The node performs GPU-accelerated raytracing on the given scene. - * Fields to be computed will be automatically determined based on connected FormatNodes and YieldPointsNodes - * Graph input: rays - * Graph output: point cloud (sparse) - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param scene Handle to a scene to perform raytracing on. Pass null to use the default scene - * @param range Maximum distance to travel for every ray - */ -RGL_API rgl_status_t -rgl_node_raytrace(rgl_node_t* node, rgl_scene_t scene, float range); - -/** - * Creates or modifies FormatNode. - * The node converts internal representation into a binary format defined by `fields` array. - * Note: It is a user's responsibility to ensure proper data structure alignment. See (https://en.wikipedia.org/wiki/Data_structure_alignment). - * Graph input: point cloud - * Graph output: point cloud - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param fields Subsequent fields to be present in the binary output - * @param field_count Number of elements in the `fields` array - */ -RGL_API rgl_status_t -rgl_node_points_format(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count); - -/** - * Creates or modifies YieldPointsNode. - * The node is a marker what fields are expected by the user. - * Graph input: point cloud - * Graph output: point cloud - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param fields Subsequent fields expected to be available - * @param field_count Number of elements in the `fields` array - */ -RGL_API rgl_status_t -rgl_node_points_yield(rgl_node_t* node, const rgl_field_t* fields, int32_t field_count); - -/** - * Creates or modifies CompactNode. - * The node removes non-hit points. In other words, it converts a point cloud into a dense one. - * Graph input: point cloud - * Graph output: point cloud (compacted) - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - */ -RGL_API rgl_status_t -rgl_node_points_compact(rgl_node_t* node); - -/** - * Creates or modifies DownSampleNode. - * The node uses VoxelGrid down-sampling filter from PCL library to reduce the number of points. - * Graph input: point cloud - * Graph output: point cloud (downsampled) - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param leaf_size_* Dimensions of the leaf voxel passed to VoxelGrid filter. - */ -RGL_API rgl_status_t -rgl_node_points_downsample(rgl_node_t* node, float leaf_size_x, float leaf_size_y, float leaf_size_z); - -/** - * Creates or modifies WritePCDFileNode. - * The node accumulates (merges) point clouds on each run. On destruction, it saves it to the given file. - * Graph input: point cloud - * Graph output: none - * @param node If (*node) == nullptr, a new node will be created. Otherwise, (*node) will be modified. - * @param file_path Path to the output pcd file. - */ -RGL_API rgl_status_t -rgl_node_points_write_pcd_file(rgl_node_t* node, const char* file_path); - -/******************************** GRAPH ********************************/ - -/** - * Starts execution of the RGL graph containing provided node. - * This function is asynchronous. - * @param node Any node from the graph to execute - */ -RGL_API rgl_status_t -rgl_graph_run(rgl_node_t node); - -/** - * Destroys RGL graph (all connected nodes) containing provided node. - * @param node Any node from the graph to destroy - */ -RGL_API rgl_status_t -rgl_graph_destroy(rgl_node_t node); - -/** - * Obtains the result information of any node in the graph. - * The function will fill output parameters that are not null. - * I.e. The count of the output elements can be queried using a nullptr out_size_of. - * @param node Node to get output from - * @param field Field to get output from. Formatted output with FormatNode should be marked as RGL_FIELD_DYNAMIC_FORMAT. - * @param out_count Returns the number of available elements (e.g. points). May be null. - * @param out_size_of Returns byte size of a single element (e.g. point). May be null. - */ -RGL_API rgl_status_t -rgl_graph_get_result_size(rgl_node_t node, rgl_field_t field, int32_t* out_count, int32_t* out_size_of); - -/** - * Obtains the result data of any node in the graph. - * If the result is not yet available, this function will block. - * @param node Node to get output from - * @param field Field to get output from. Formatted output with FormatNode should be marked as RGL_FIELD_DYNAMIC_FORMAT. - * @param data Returns binary data, expects a buffer of size (*out_count) * (*out_size_of) from rgl_graph_get_result_size(...) call. - */ -RGL_API rgl_status_t -rgl_graph_get_result_data(rgl_node_t node, rgl_field_t field, void* data); - -/** - * Activates or deactivates node in the graph. - * Children of inactive nodes do not execute as well. - * Nodes are active by default. - * @param node Node to activate/deactivate. - * @param active Whether node active or not. - */ -RGL_API rgl_status_t -rgl_graph_node_set_active(rgl_node_t node, bool active); - -/** - * Adds child to the parent node - * @param parent Node that will be set as the parent of (child) - * @param child Node that will be set as the child of (parent) - */ -RGL_API rgl_status_t -rgl_graph_node_add_child(rgl_node_t parent, rgl_node_t child); - -/** - * Removes child from the parent node - * @param parent Node that will be removed as parent from (child) - * @param child Node that will be removed as child from (parent) - */ -RGL_API rgl_status_t -rgl_graph_node_remove_child(rgl_node_t parent, rgl_node_t child); diff --git a/RGLServerPlugin/src/Lidar.cc b/RGLServerPlugin/src/Lidar.cc index 8d6d208..7367f45 100644 --- a/RGLServerPlugin/src/Lidar.cc +++ b/RGLServerPlugin/src/Lidar.cc @@ -36,10 +36,6 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptrHasElement(PARAM_RANGE_ID)) { - gzerr << "No '" << PARAM_RANGE_ID << "' parameter specified for the RGL lidar. Disabling plugin.\n"; -======= if (!sdf->HasElement(PARAM_RANGE_ID) || !sdf->FindElement(PARAM_RANGE_ID)->HasElement(PARAM_RANGE_MIN_ID) || !sdf->FindElement(PARAM_RANGE_ID)->HasElement(PARAM_RANGE_MAX_ID)) { @@ -48,7 +44,6 @@ bool RGLServerPluginInstance::LoadConfiguration(const std::shared_ptr" << "\n" << " <" << PARAM_RANGE_MAX_ID << ">" << "\n" << "\n"; ->>>>>>> main return false; } @@ -232,10 +227,6 @@ void RGLServerPluginInstance::RayTrace(std::chrono::steady_clock::duration simTi } } -<<<<<<< HEAD -gz::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) -{ -======= bool RGLServerPluginInstance::FetchLaserScanResult() { if (!CheckRGL(rgl_graph_get_result_data(rglNodeYieldLaserScan, RGL_FIELD_DISTANCE_F32, resultLaserScan.distances.data())) || @@ -261,7 +252,6 @@ bool RGLServerPluginInstance::FetchPointCloudResult(rgl_node_t formatNode) gz::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::steady_clock::duration simTime, const std::string& frame) { auto pointCount = resultLaserScan.distances.size(); ->>>>>>> main gz::msgs::LaserScan outMsg; *outMsg.mutable_header()->mutable_stamp() = gz::msgs::Convert(simTime); auto _frame = outMsg.mutable_header()->add_data(); @@ -288,14 +278,6 @@ gz::msgs::LaserScan RGLServerPluginInstance::CreateLaserScanMsg(std::chrono::ste return outMsg; } -<<<<<<< HEAD -gz::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chrono::steady_clock::duration simTime, std::string frame, int hitpointCount) -{ - gz::msgs::PointCloudPacked outMsg; - gz::msgs::InitPointCloudPacked(outMsg, frame, false, - {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - outMsg.mutable_data()->resize(hitpointCount * outMsg.point_step()); -======= gz::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chrono::steady_clock::duration simTime, const std::string& frame) { gz::msgs::PointCloudPacked outMsg; @@ -303,19 +285,13 @@ gz::msgs::PointCloudPacked RGLServerPluginInstance::CreatePointCloudMsg(std::chr {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}, {"intensity",gz::msgs::PointCloudPacked::Field::FLOAT32}}); outMsg.mutable_data()->resize(resultPointCloud.hitPointCount * outMsg.point_step()); ->>>>>>> main *outMsg.mutable_header()->mutable_stamp() = gz::msgs::Convert(simTime); outMsg.set_height(1); outMsg.set_width(resultPointCloud.hitPointCount); outMsg.set_row_step(resultPointCloud.hitPointCount * outMsg.point_step()); -<<<<<<< HEAD - gz::msgs::PointCloudPackedIterator xIterWorld(outMsg, "x"); - memcpy(&(*xIterWorld), resultPointCloud.data(), hitpointCount * sizeof(rgl_vec3f)); -======= gz::msgs::PointCloudPackedIterator xIter(outMsg, "x"); memcpy(&(*xIter), resultPointCloud.data.data(), resultPointCloud.hitPointCount * resultPointCloud.pointSize); ->>>>>>> main return outMsg; } From db6e6e0be4a575388407f27b778d8d56d6497054 Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Fri, 3 Jan 2025 18:20:16 +0000 Subject: [PATCH 09/10] Update readme --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 4ed61bf..e568f29 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ Key features: - OS: [Ubuntu 22.04](https://releases.ubuntu.com/jammy/) or [Ubuntu 24.04](https://releases.ubuntu.com/noble/) -- Gazebo: [Harmonic](https://gazebosim.org/docs/harmonic/install) +- Gazebo: [Ionic](https://gazebosim.org/docs/ionic/install/) - GPU: CUDA-enabled From 13f22b7e9a1f373fc4f67249cb909236e76036cb Mon Sep 17 00:00:00 2001 From: Wiktor Bajor Date: Sun, 19 Jan 2025 15:13:51 +0100 Subject: [PATCH 10/10] Use gazebo vendor pkgs --- RGLServerPlugin/CMakeLists.txt | 19 ++++++++----------- RGLVisualize/CMakeLists.txt | 22 +++++++++------------- 2 files changed, 17 insertions(+), 24 deletions(-) diff --git a/RGLServerPlugin/CMakeLists.txt b/RGLServerPlugin/CMakeLists.txt index 9646a54..e86214c 100644 --- a/RGLServerPlugin/CMakeLists.txt +++ b/RGLServerPlugin/CMakeLists.txt @@ -2,21 +2,18 @@ cmake_minimum_required(VERSION 3.21) project(RGLServerPlugin) set(CMAKE_CXX_STANDARD 20) -find_package(gz-cmake4 REQUIRED) -gz_find_package(gz-plugin3 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${gz-plugin3_VERSION_MAJOR}) - -gz_find_package(gz-sim9 REQUIRED) -set(GZ_SIM_VER ${gz-sim9_VERSION_MAJOR}) - +find_package(gz_plugin_vendor REQUIRED) +find_package(gz-plugin REQUIRED) +find_package(gz_sim_vendor REQUIRED) +find_package(gz-sim REQUIRED) include_directories(include) add_library(RGLServerPluginManager SHARED src/RGLServerPluginManager.cc src/Mesh.cc src/Utils.cc src/Scene.cc) target_include_directories(RGLServerPluginManager PRIVATE RobotecGPULidar) target_link_libraries(RGLServerPluginManager - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + PRIVATE gz-plugin::gz-plugin + PRIVATE gz-sim::gz-sim PRIVATE RobotecGPULidar ) set_target_properties(RGLServerPluginManager PROPERTIES INSTALL_RPATH "$ORIGIN") @@ -24,8 +21,8 @@ set_target_properties(RGLServerPluginManager PROPERTIES INSTALL_RPATH "$ORIGIN") add_library(RGLServerPluginInstance SHARED src/RGLServerPluginInstance.cc src/Lidar.cc src/Utils.cc src/LidarPatternLoader.cc) target_include_directories(RGLServerPluginInstance PRIVATE RobotecGPULidar) target_link_libraries(RGLServerPluginInstance - PRIVATE gz-plugin${GZ_PLUGIN_VER}::gz-plugin${GZ_PLUGIN_VER} - PRIVATE gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} + PRIVATE gz-plugin::gz-plugin + PRIVATE gz-sim::gz-sim PRIVATE RobotecGPULidar ) set_target_properties(RGLServerPluginInstance PROPERTIES INSTALL_RPATH "$ORIGIN") diff --git a/RGLVisualize/CMakeLists.txt b/RGLVisualize/CMakeLists.txt index f5c6411..3269c3e 100644 --- a/RGLVisualize/CMakeLists.txt +++ b/RGLVisualize/CMakeLists.txt @@ -9,16 +9,12 @@ project(RGLVisualize) set(CMAKE_AUTOMOC ON) -find_package(gz-cmake4 REQUIRED) -gz_find_package(gz-msgs11 REQUIRED) -set(GZ_MSGS_VER ${gz-msgs11_VERSION_MAJOR}) - -gz_find_package(gz-rendering9 REQUIRED) -set(GZ_RENDERING_VER ${gz-rendering9_VERSION_MAJOR}) - - -gz_find_package(gz-gui9 REQUIRED) -set(GZ_GUI_VER ${gz-gui9_VERSION_MAJOR}) +find_package(gz_msgs_vendor REQUIRED) +find_package(gz-msgs REQUIRED) +find_package(gz_rendering_vendor REQUIRED) +find_package(gz-rendering REQUIRED) +find_package(gz_gui_vendor REQUIRED) +find_package(gz-gui REQUIRED) QT5_ADD_RESOURCES(resources_RCC RGLVisualize.qrc) @@ -29,9 +25,9 @@ add_library(RGLVisualize SHARED target_link_libraries(RGLVisualize PRIVATE - gz-msgs${GZ_MSGS_VER}::gz-msgs${GZ_MSGS_VER} - gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} - gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER} + gz-msgs::gz-msgs + gz-rendering::gz-rendering + gz-gui::gz-gui ) ## Install libraries