diff --git a/include/gz/sim/components/RaycastData.hh b/include/gz/sim/components/RaycastData.hh new file mode 100644 index 0000000000..ab776695a8 --- /dev/null +++ b/include/gz/sim/components/RaycastData.hh @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2024 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#ifndef GZ_SIM_COMPONENTS_RAYCASTDATA_HH_ +#define GZ_SIM_COMPONENTS_RAYCASTDATA_HH_ + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ +/// \brief A struct that holds the information of a ray. +struct RayInfo +{ + /// \brief Starting point of the ray in entity frame + gz::math::Vector3d start; + + /// \brief Ending point of the ray in entity frame + gz::math::Vector3d end; +}; + +/// \brief A struct that holds the result of a raycasting operation. +struct RaycastResultInfo +{ + /// \brief The hit point in entity frame + gz::math::Vector3d point; + + /// \brief The fraction of the ray length at the intersection/hit point. + double fraction; + + /// \brief The normal at the hit point in entity frame + gz::math::Vector3d normal; +}; + +/// @brief A struct that holds the raycasting data, including ray and results +struct RaycastDataInfo +{ + /// @brief The rays to cast from the entity. + std::vector rays; + + /// @brief The results of the raycasting. + std::vector results; +}; +} + +namespace serializers +{ + /// \brief Specialization of DefaultSerializer for RaycastDataInfo + template<> class DefaultSerializer + { + public: static std::ostream &Serialize( + std::ostream &_out, const components::RaycastDataInfo &) + { + return _out; + } + + public: static std::istream &Deserialize( + std::istream &_in, components::RaycastDataInfo &) + { + return _in; + } + }; +} + +namespace components +{ +/// \brief A component type that contains the rays traced from an entity +/// into a physics world, along with the results of the raycasting operation. +/// +/// This component is primarily used for applications that require raycasting. +/// The target application defines the rays, and the physics system plugin +/// updates the raycasting results during each update loop. +using RaycastData = Component>; + +GZ_SIM_REGISTER_COMPONENT("gz_sim_components.RaycastData", RaycastData) +} +} +} +} +#endif // GZ_SIM_COMPONENTS_RAYCASTDATA_HH_ diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index 2a2bb721ca..f0e9edec2f 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -17,13 +17,11 @@ #include "VisualizeLidar.hh" +#include #include #include #include -#include -#include - #include #include @@ -78,18 +76,11 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Pointer to LidarVisual public: rendering::LidarVisualPtr lidar; - /// \brief Visual type for lidar visual - public: rendering::LidarVisualType visualType{ - rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; - - /// \brief LaserScan message from sensor - public: msgs::LaserScan msg; - /// \brief Pose of the lidar visual public: math::Pose3d lidarPose{math::Pose3d::Zero}; /// \brief Topic name to subscribe - public: std::string topicName{""}; + public: std::string topicName; /// \brief List of topics publishing LaserScan messages. public: QStringList topicList; @@ -109,6 +100,10 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// maxVisualRange public: std::mutex serviceMutex; + /// \brief Visual type for lidar visual + public: rendering::LidarVisualType visualType{ + rendering::LidarVisualType::LVT_TRIANGLE_STRIPS}; + /// \brief Initialization flag public: bool initialized{false}; @@ -145,42 +140,9 @@ VisualizeLidar::~VisualizeLidar() ///////////////////////////////////////////////// void VisualizeLidar::LoadLidar() { - auto loadedEngNames = rendering::loadedEngines(); - if (loadedEngNames.empty()) - return; - - // assume there is only one engine loaded - auto engineName = loadedEngNames[0]; - if (loadedEngNames.size() > 1) - { - gzdbg << "More than one engine is available. " - << "VisualizeLidar plugin will use engine [" - << engineName << "]" << std::endl; - } - auto engine = rendering::engine(engineName); - if (!engine) - { - gzerr << "Internal error: failed to load engine [" << engineName - << "]. VisualizeLidar plugin won't work." << std::endl; - return; - } - - if (engine->SceneCount() == 0) - return; - - // assume there is only one scene - // load scene - auto scene = engine->SceneByIndex(0); + auto scene = rendering::sceneFromFirstRenderEngine(); if (!scene) - { - gzerr << "Internal error: scene is null." << std::endl; - return; - } - - if (!scene->IsInitialized()) - { return; - } // Create lidar visual gzdbg << "Creating lidar visual" << std::endl; @@ -190,7 +152,7 @@ void VisualizeLidar::LoadLidar() if (!this->dataPtr->lidar) { gzwarn << "Failed to create lidar, visualize lidar plugin won't work." - << std::endl; + << std::endl; scene->DestroyVisual(this->dataPtr->lidar); @@ -363,7 +325,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) !this->dataPtr->node.Unsubscribe(this->dataPtr->topicName)) { gzerr << "Unable to unsubscribe from topic [" - << this->dataPtr->topicName <<"]" <dataPtr->topicName <<"]" <dataPtr->topicName = topic; @@ -376,7 +338,7 @@ void VisualizeLidar::OnTopic(const QString &_topicName) &VisualizeLidar::OnScan, this)) { gzerr << "Unable to subscribe to topic [" - << this->dataPtr->topicName << "]\n"; + << this->dataPtr->topicName << "]\n"; return; } this->dataPtr->visualDirty = false; @@ -398,7 +360,7 @@ void VisualizeLidar::DisplayVisual(bool _value) std::lock_guard lock(this->dataPtr->serviceMutex); this->dataPtr->lidar->SetVisible(_value); gzmsg << "Lidar Visual Display " << ((_value) ? "ON." : "OFF.") - << std::endl; + << std::endl; } ///////////////////////////////////////////////// @@ -412,12 +374,12 @@ void VisualizeLidar::OnRefresh() // Get updated list std::vector allTopics; this->dataPtr->node.TopicList(allTopics); - for (auto topic : allTopics) + for (const auto &topic : allTopics) { std::vector publishers; std::vector subscribers; this->dataPtr->node.TopicInfo(topic, publishers, subscribers); - for (auto pub : publishers) + for (const auto &pub : publishers) { if (pub.MsgTypeName() == "gz.msgs.LaserScan") { @@ -426,7 +388,7 @@ void VisualizeLidar::OnRefresh() } } } - if (this->dataPtr->topicList.size() > 0) + if (!this->dataPtr->topicList.empty()) { this->OnTopic(this->dataPtr->topicList.at(0)); } @@ -453,35 +415,26 @@ void VisualizeLidar::OnScan(const msgs::LaserScan &_msg) std::lock_guard lock(this->dataPtr->serviceMutex); if (this->dataPtr->initialized) { - this->dataPtr->msg = std::move(_msg); - this->dataPtr->lidar->SetVerticalRayCount( - this->dataPtr->msg.vertical_count()); - this->dataPtr->lidar->SetHorizontalRayCount( - this->dataPtr->msg.count()); - this->dataPtr->lidar->SetMinHorizontalAngle( - this->dataPtr->msg.angle_min()); - this->dataPtr->lidar->SetMaxHorizontalAngle( - this->dataPtr->msg.angle_max()); - this->dataPtr->lidar->SetMinVerticalAngle( - this->dataPtr->msg.vertical_angle_min()); - this->dataPtr->lidar->SetMaxVerticalAngle( - this->dataPtr->msg.vertical_angle_max()); + this->dataPtr->lidar->SetVerticalRayCount(_msg.vertical_count()); + this->dataPtr->lidar->SetHorizontalRayCount(_msg.count()); + this->dataPtr->lidar->SetMinHorizontalAngle(_msg.angle_min()); + this->dataPtr->lidar->SetMaxHorizontalAngle(_msg.angle_max()); + this->dataPtr->lidar->SetMinVerticalAngle(_msg.vertical_angle_min()); + this->dataPtr->lidar->SetMaxVerticalAngle(_msg.vertical_angle_max()); this->dataPtr->lidar->SetPoints(std::vector( - this->dataPtr->msg.ranges().begin(), - this->dataPtr->msg.ranges().end())); + _msg.ranges().begin(), + _msg.ranges().end())); - if (!math::equal(this->dataPtr->maxVisualRange, - this->dataPtr->msg.range_max())) + if (!math::equal(this->dataPtr->maxVisualRange, _msg.range_max())) { - this->dataPtr->maxVisualRange = this->dataPtr->msg.range_max(); + this->dataPtr->maxVisualRange = _msg.range_max(); this->dataPtr->lidar->SetMaxRange(this->dataPtr->maxVisualRange); this->MaxRangeChanged(); } - if (!math::equal(this->dataPtr->minVisualRange, - this->dataPtr->msg.range_min())) + if (!math::equal(this->dataPtr->minVisualRange, _msg.range_min())) { - this->dataPtr->minVisualRange = this->dataPtr->msg.range_min(); + this->dataPtr->minVisualRange = _msg.range_min(); this->dataPtr->lidar->SetMinRange(this->dataPtr->minVisualRange); this->MinRangeChanged(); } diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh index 6841a929c7..572304b93a 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.hh +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.hh @@ -35,7 +35,7 @@ inline namespace GZ_SIM_VERSION_NAMESPACE /// \brief Visualize the LaserScan message returned by the sensors. Use the /// checkbox to turn visualization of non-hitting rays on or off and - /// the textfield to select the message to be visualised. The combobox is + /// the textfield to select the message to be visualized. The combobox is /// used to select the type of visual for the sensor data. class VisualizeLidar : public gz::sim::GuiSystem { diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 0bb135048d..e37551c7a0 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -132,6 +133,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParentLinkName.hh" +#include "gz/sim/components/RaycastData.hh" #include "gz/sim/components/ExternalWorldWrenchCmd.hh" #include "gz/sim/components/JointTransmittedWrench.hh" #include "gz/sim/components/JointForceCmd.hh" @@ -314,6 +316,10 @@ class gz::sim::systems::PhysicsPrivate /// \param[in] _ecm Mutable reference to ECM. public: void UpdateCollisions(EntityComponentManager &_ecm); + /// \brief Update ray intersection components from physics simulation + /// \param[in] _ecm Mutable reference to ECM. + public: void UpdateRayIntersections(EntityComponentManager &_ecm); + /// \brief FrameData relative to world at a given offset pose /// \param[in] _link gz-physics link /// \param[in] _pose Offset pose in which to compute the frame data @@ -514,6 +520,11 @@ class gz::sim::systems::PhysicsPrivate CollisionFeatureList, physics::GetContactsFromLastStepFeature>{}; + /// \brief Feature list to handle ray intersection information. + public: struct RayIntersectionFeatureList : physics::FeatureList< + MinimumFeatureList, + physics::GetRayIntersectionFromLastStepFeature>{}; + /// \brief Feature list to change contacts before they are applied to physics. public: struct SetContactPropertiesCallbackFeatureList : physics::FeatureList< @@ -651,6 +662,7 @@ class gz::sim::systems::PhysicsPrivate MinimumFeatureList, CollisionFeatureList, ContactFeatureList, + RayIntersectionFeatureList, SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, @@ -3984,6 +3996,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); + + this->UpdateRayIntersections(_ecm); } // NOLINT readability/fn_size // TODO (azeey) Reduce size of function and remove the NOLINT above @@ -4165,6 +4179,97 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) }); } +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateRayIntersections(EntityComponentManager &_ecm) +{ + GZ_PROFILE("PhysicsPrivate::UpdateRayIntersections"); + // Quit early if the RaycastData component hasn't been created. + // This means there are no systems that need raycasting information + if (!_ecm.HasComponentType(components::RaycastData::typeId)) + return; + + // Assume that there is only one world entity + Entity worldEntity = _ecm.EntityByComponents(components::World()); + + if (!this->entityWorldMap.HasEntity(worldEntity)) + { + gzwarn << "Failed to find world [" << worldEntity << "]." << std::endl; + return; + } + + auto worldRayIntersectionFeature = + this->entityWorldMap.EntityCast(worldEntity); + + if (!worldRayIntersectionFeature) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting process ray intersections, but the physics " + << "engine doesn't support ray intersection features. " + << "Ray intersections won't be computed." + << std::endl; + informed = true; + } + return; + } + + // Go through each entity that has a RaycastData components, trace the + // rays and store the results + _ecm.Each( + [&](const Entity &_entity, + components::RaycastData *_raycastData, + components::Pose */*_pose*/) -> bool + { + // Retrieve the rays from the RaycastData component + const auto &rays = _raycastData->Data().rays; + + // Clear the previous results + auto &results = _raycastData->Data().results; + results.clear(); + results.reserve(rays.size()); + + // Get the entity's world pose + const auto &entityWorldPose = worldPose(_entity, _ecm); + + for (const auto &ray : rays) + { + // Convert ray to world frame + const math::Vector3d rayStart = entityWorldPose.Pos() + + entityWorldPose.Rot().RotateVector(ray.start); + const math::Vector3d rayEnd = entityWorldPose.Pos() + + entityWorldPose.Rot().RotateVector(ray.end); + + // Perform ray intersection + auto rayIntersection = + worldRayIntersectionFeature->GetRayIntersectionFromLastStep( + math::eigen3::convert(rayStart), + math::eigen3::convert(rayEnd)); + + const auto rayIntersectionResult = + rayIntersection.Get< + physics::World3d::RayIntersection>(); + + results.emplace_back(); + auto &result = results.back(); + + // Convert result to entity frame and store + const math::Vector3d intersectionPoint = + math::eigen3::convert(rayIntersectionResult.point); + result.point = entityWorldPose.Rot().RotateVectorReverse( + intersectionPoint - entityWorldPose.Pos()); + + result.fraction = rayIntersectionResult.fraction; + + const math::Vector3d normal = + math::eigen3::convert(rayIntersectionResult.normal); + result.normal = entityWorldPose.Rot().RotateVectorReverse(normal); + } + return true; + }); +} + ////////////////////////////////////////////////// physics::FrameData3d PhysicsPrivate::LinkFrameDataAtOffset( const LinkPtrType &_link, const math::Pose3d &_pose) const diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index 7a50c8c7db..f487e1b289 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -37,6 +37,8 @@ #include #include +#include + #include "gz/sim/Entity.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Link.hh" @@ -76,6 +78,7 @@ #include "gz/sim/components/Physics.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/PoseCmd.hh" +#include "gz/sim/components/RaycastData.hh" #include "gz/sim/components/Static.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/World.hh" @@ -2904,3 +2907,105 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld)) server.AddSystem(testSystem.systemPtr); server.Run(true, 1000, false); } + +////////////////////////////////////////////////// +/// Test ray intersections computed by physics system during Update loop. +TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(RayIntersections)) +{ + ServerConfig serverConfig; + + const auto sdfFile = + common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "empty.sdf"); + + serverConfig.SetSdfFile(sdfFile); + Server server(serverConfig); + server.SetUpdatePeriod(1ns); + + Entity testEntity1; + Entity testEntity2; + + test::Relay testSystem; + + // During PreUpdate, add rays to testEntity1 and testEntity2 + testSystem.OnPreUpdate( + [&](const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) + { + // Set collision detector to bullet (supports ray intersections). + auto worldEntity = _ecm.EntityByComponents(components::World()); + _ecm.CreateComponent( + worldEntity, components::PhysicsCollisionDetector("bullet")); + + // Create RaycastData component for testEntity1 + testEntity1 = _ecm.CreateEntity(); + _ecm.CreateComponent(testEntity1, components::RaycastData()); + _ecm.CreateComponent( + testEntity1, components::Pose(math::Pose3d(0, 0, 10, 0, 0, 0))); + + // Create RaycastData component for testEntity2 + testEntity2 = _ecm.CreateEntity(); + _ecm.CreateComponent(testEntity2, components::RaycastData()); + _ecm.CreateComponent( + testEntity2, components::Pose(math::Pose3d(0, 0, 10, 0, 0, 0))); + + // Add 5 rays to testEntity1 that intersect with the ground plane + auto &rays1 = + _ecm.Component(testEntity1)->Data().rays; + for (int i = 0; i < 5; ++i) + { + components::RayInfo ray; + ray.start = math::Vector3d(0, 0, -i); + ray.end = math::Vector3d(0, 0, -20); + rays1.push_back(ray); + } + + // Add 2 rays to testEntity2 that don't intersect with the ground plane + auto &rays2 = + _ecm.Component(testEntity2)->Data().rays; + for (int i = 0; i < 2; ++i) + { + components::RayInfo ray; + ray.start = math::Vector3d(0, 0, -i); + ray.end = math::Vector3d(0, 0, 5); + rays2.push_back(ray); + } + }); + // Check ray intersections for testEntity1 and testEntity2 + testSystem.OnPostUpdate( + [&](const UpdateInfo &/*_info*/, const EntityComponentManager &_ecm) + { + // check the raycasting results for testEntity1 + auto &rays1 = + _ecm.Component(testEntity1)->Data().rays; + auto &results1 = + _ecm.Component(testEntity1)->Data().results; + ASSERT_EQ(rays1.size(), results1.size()); + + for (size_t i = 0; i < results1.size(); ++i) { + ASSERT_EQ(results1[i].point, math::Vector3d(0, 0, -10)); + ASSERT_EQ(results1[i].normal, math::Vector3d(0, 0, 1)); + const double expFraction = + (rays1[i].start - results1[i].point).Length() / + (rays1[i].start - rays1[i].end).Length(); + ASSERT_NEAR(results1[i].fraction, expFraction, 1e-6); + } + + // check the raycasting results for testEntity2 + auto &rays2 = + _ecm.Component(testEntity2)->Data().rays; + auto &results2 = + _ecm.Component(testEntity2)->Data().results; + ASSERT_EQ(rays2.size(), results2.size()); + + for (size_t i = 0; i < results2.size(); ++i) { + ASSERT_TRUE( + math::eigen3::convert(results2[i].point).array().isNaN().all()); + ASSERT_TRUE( + math::eigen3::convert(results2[i].normal).array().isNaN().all()); + ASSERT_TRUE( + std::isnan(results2[i].fraction)); + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1, false); +}