Merge pull request #64 from naturerobots/feature/configurable-cost-update-refresh-rate

Author: JustusBraun

rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp

@@ -339,6 +339,11 @@ private Q_SLOTS:
    */
   void updateCullingMode();
 
+  /**
+   * @brief Updates the vertex cost update application frequency
+   */
+  void updateVertexCostUpdateFrequency();
+
 private:
   /**
    * @brief RViz callback on initialize
@@ -455,7 +460,8 @@ private Q_SLOTS:
 
   /// Cache for batching cost updates
   ThreadSafeQueue<mesh_msgs::msg::MeshVertexCostsSparseStamped::ConstSharedPtr> m_costsUpdateCache;
-  float m_timeSinceLastUpdateApply;
+  std::chrono::nanoseconds m_timeSinceLastUpdateApply;
+  std::chrono::nanoseconds m_invUpdateFreq;
 
 
   /// Counter for the received messages
@@ -517,6 +523,9 @@ private Q_SLOTS:
   rviz_common::properties::QosProfileProperty* m_vertexCostUpdateTopicQos;
   rclcpp::QoS m_vertexCostUpdateQos;
 
+  /// Property to set the update processing frequency
+  rviz_common::properties::IntProperty* m_vertexCostsRefreshRate;
+
   /// Property to select different types of vertex cost maps to be shown
   rviz_common::properties::EnumProperty* m_selectVertexCostMap;
 

rviz_mesh_tools_plugins/src/MeshDisplay.cpp

@@ -104,7 +104,8 @@ namespace rviz_mesh_tools_plugins
 MeshDisplay::MeshDisplay() 
 : rviz_common::Display()
 , m_ignoreMsgs(false)
-, m_timeSinceLastUpdateApply(0.0)
+, m_timeSinceLastUpdateApply(0)
+, m_invUpdateFreq(std::chrono::nanoseconds(long(1e9)) / 10)
 , m_meshQos(rclcpp::QoS(1).transient_local())
 , m_vertexColorsQos(rclcpp::QoS(5))
 , m_vertexCostsQos(rclcpp::QoS(5))
@@ -200,6 +201,15 @@ MeshDisplay::MeshDisplay()
       );
       m_vertexCostUpdateTopicQos = new rviz_common::properties::QosProfileProperty(m_vertexCostUpdateTopic, m_vertexCostUpdateQos);
 
+      m_vertexCostsRefreshRate = new rviz_common::properties::IntProperty(
+        "Visualization Update Rate", 10,
+        "Sets the frequency (Hz) with which vertex cost updates are applied to the visual.",
+        m_displayType,
+        SLOT(updateVertexCostUpdateFrequency()),
+        this,
+        1  // Min value: This is 1 since we want at least one update per second
+      );
+
       m_selectVertexCostMap = new rviz_common::properties::EnumProperty("Vertex Costs Type", "-- None --",
                                                      "Select the type of vertex cost map to be displayed. New types "
                                                      "will appear here when a new message arrives.",
@@ -320,16 +330,18 @@ void MeshDisplay::onInitialize()
 
 void MeshDisplay::update(float wall_dt, float ros_dt) 
 {
+  // NOTE: In rolling the update function uses std::chrono::nanoseconds instead of float
   (void) ros_dt;
 
-  if (m_timeSinceLastUpdateApply >= 1.0e9)
+  // Determine current configured update freq
+  if (m_timeSinceLastUpdateApply >= m_invUpdateFreq)
   {
     this->applyCachedCostUpdates();
-    m_timeSinceLastUpdateApply = 0.0;
+    m_timeSinceLastUpdateApply = std::chrono::nanoseconds(0);
   }
   else
   {
-    m_timeSinceLastUpdateApply += wall_dt;
+    m_timeSinceLastUpdateApply += std::chrono::nanoseconds(long(wall_dt));
   }
 
   this->transformMesh();
@@ -634,6 +646,8 @@ void MeshDisplay::updateDisplayType()
   m_textureServiceName->hide();
   m_costColorType->hide();
   m_vertexCostsTopic->hide();
+  m_vertexCostUpdateTopic->hide();
+  m_vertexCostsRefreshRate->hide();
   m_selectVertexCostMap->hide();
   m_costUseCustomLimits->hide();
   m_costLowerLimit->hide();
@@ -669,11 +683,13 @@ void MeshDisplay::updateDisplayType()
       if (!m_ignoreMsgs)
       {
         m_vertexCostsTopic->show();
+        m_vertexCostUpdateTopic->show();
         if (!m_costsMsgCache) // checks whether we are not subscribed yet, avoids resetting the subscription when something else changes in the display type
         {
           updateVertexCostsSubscription();
         }
       }
+      m_vertexCostsRefreshRate->show();
       m_selectVertexCostMap->show();
       m_costUseCustomLimits->show();
       if (m_costUseCustomLimits->getBool())
@@ -944,6 +960,33 @@ void MeshDisplay::updateCullingMode()
   emit signalCullingModeChanged(static_cast<Ogre::CullingMode>(m_cullingMode->getOptionInt()));
 }
 
+void MeshDisplay::updateVertexCostUpdateFrequency()
+{
+  const int freq = m_vertexCostsRefreshRate->getInt();
+
+  // IntProperty::getInt returns 0 if invalid data was entered
+  // We set the min value to 1 so a 0 return is always an error
+  if (0 == freq)
+  {
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rviz_mesh_tools_plugins"),
+      "[MeshDisplay::updateVertexCostUpdateFrequency] Invalid update frequency entered!"
+    );
+    m_vertexCostsRefreshRate->setInt(1);
+    m_invUpdateFreq = std::chrono::seconds(1);
+    return;
+  }
+
+  using namespace std::chrono_literals;
+  m_invUpdateFreq = std::chrono::nanoseconds(1s) / freq;
+
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("rviz_mesh_tools_plugins"),
+    "[MeshDisplay::updateVertexCostUpdateFrequency] Update frequency set to %i, delta t is %lu nanoseconds",
+    freq, m_invUpdateFreq.count()
+  );
+}
+
 // =====================================================================================================================
 // Data loading
 void MeshDisplay::initialServiceCall()

rviz_mesh_tools_plugins/src/MeshVisual.cpp

@@ -1089,36 +1089,31 @@ bool MeshVisual::updateVertexCosts(
   }
 
   /* ManualObject::getNumSections and ManualObject::getSection are deprecated
-   * in the Ogre version ROS Jazzy (and future versions) uses. Since the new
-   * API is not available in the Ogre version used by ROS Humble we use this
-   * check to keep Humble support.
+   * in the Ogre version ROS Jazzy (and future versions) uses. The VES_COLOUR
+   * semantic is also new in the ROS Jazzy Ogre version. Prior versions use
+   * VES_DIFFUSE for colours in the ManualObject. Since the new API is not
+   * available in the Ogre version used by ROS Humble we use this check to keep
+   * Humble support.
    *
    * This can be removed when Humble support is dropped.
    */
 #if OGRE_VERSION < ((1 << 16) | (12 << 8) | 7)
   Ogre::RenderOperation* render_op = m_vertexCostsMesh->getSection(0)->getRenderOperation();
   const Ogre::VertexDeclaration* v_decl = render_op->vertexData->vertexDeclaration;
-
-  // findElementBySemantic does not support VES_COLOUR yet
-  const Ogre::VertexElement* color_sem = nullptr;
-  for (const auto& elem: v_decl->getElements())
-  {
-    // VET_COLOUR is deprecated in favour of VET_UBYTE4_NORM
-    if (Ogre::VET_UBYTE4_NORM == elem.getType())
-    {
-      color_sem = &elem;
-    }
-  }
+  const Ogre::VertexElement* color_elem = v_decl->findElementBySemantic(Ogre::VES_DIFFUSE);
 #else
   // Get the needed Vertex Colour attribute information about the raw vertex buffer
   Ogre::RenderOperation* render_op = m_vertexCostsMesh->getSections().front()->getRenderOperation();
   const Ogre::VertexDeclaration* v_decl = render_op->vertexData->vertexDeclaration;
-  const Ogre::VertexElement* color_sem = v_decl->findElementBySemantic(Ogre::VES_COLOUR);
+  const Ogre::VertexElement* color_elem = v_decl->findElementBySemantic(Ogre::VES_COLOUR);
 #endif
 
-  if (nullptr == color_sem)
+  if (nullptr == color_elem)
   {
-    RCLCPP_ERROR(rclcpp::get_logger("rviz_mesh_tools_plugins"), "Vertex Cost Mesh has no Vertex Colour attribute!");
+    RCLCPP_ERROR(
+      rclcpp::get_logger("rviz_mesh_tools_plugins"),
+      "Cannot update vertex costs: Vertex Cost Mesh has no Vertex Colour element!"
+    );
     return false;
   }
 
@@ -1143,7 +1138,7 @@ bool MeshVisual::updateVertexCosts(
     std::byte* vertex = raw + v * vbuf->getVertexSize();
     // Pointer to the color
     Ogre::ABGR* vcolor = nullptr;
-    color_sem->baseVertexPointerToElement(vertex, &vcolor);
+    color_elem->baseVertexPointerToElement(vertex, &vcolor);
 
     // write vertex colors that are calculated from the cost values
     float normalizedCost = (costs[i] - minCost) / range;