Merge branch 'gz-sim-executable' of github.com:sauk2/gz-sim into gz-s…

…im-executable

Signed-off-by: Saurabh Kamat <[email protected]>

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 <gz/math/Vector3.hh>
+#include <gz/sim/components/Factory.hh>
+#include <gz/sim/components/Component.hh>
+#include <gz/sim/components/Serialization.hh>
+#include <gz/sim/config.hh>
+
+#include <istream>
+#include <ostream>
+#include <vector>
+
+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<RayInfo> rays;
+
+  /// @brief The results of the raycasting.
+  std::vector<RaycastResultInfo> results;
+};
+}
+
+namespace serializers
+{
+  /// \brief Specialization of DefaultSerializer for RaycastDataInfo
+  template<> class DefaultSerializer<components::RaycastDataInfo>
+  {
+    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<RaycastDataInfo, class RaycastDataTag,
+                              serializers::DefaultSerializer<RaycastDataInfo>>;
+
+GZ_SIM_REGISTER_COMPONENT("gz_sim_components.RaycastData", RaycastData)
+}
+}
+}
+}
+#endif  // GZ_SIM_COMPONENTS_RAYCASTDATA_HH_

src/gui/plugins/visualize_lidar/VisualizeLidar.cc

@@ -17,13 +17,11 @@
 
 #include "VisualizeLidar.hh"
 
+#include <mutex>
 #include <string>
 #include <utility>
 #include <vector>
 
-#include <sdf/Link.hh>
-#include <sdf/Model.hh>
-
 #include <gz/common/Console.hh>
 #include <gz/common/Profiler.hh>
 
@@ -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 <<"]" <<std::endl;
+          << this->dataPtr->topicName <<"]" <<std::endl;
   }
   this->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<std::mutex> 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<std::string> allTopics;
   this->dataPtr->node.TopicList(allTopics);
-  for (auto topic : allTopics)
+  for (const auto &topic : allTopics)
   {
     std::vector<transport::MessagePublisher> publishers;
     std::vector<transport::MessagePublisher> 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<std::mutex> 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<double>(
-                                  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();
     }

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
   {