Merge pull request #61 from JustusBraun/feature/dynamic-cost-updates

Enable Use of New Display by Topic for MeshGeometryStamped

Author: amock

rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/MeshDisplay.hpp

@@ -85,7 +85,7 @@
 #include <message_filters/cache.h>
 
 #include <rviz_mesh_tools_plugins/RVizMessageFilter.hpp>
-#include <rviz_mesh_tools_plugins/ThreadSaveQueue.hpp>
+#include <rviz_mesh_tools_plugins/ThreadSafeQueue.hpp>
 
 #include <rviz_rendering/mesh_loader.hpp>
 
@@ -143,7 +143,7 @@ class MeshDisplay : public rviz_common::Display
   Q_OBJECT
 
 public:
-  /**#include "rviz_common/ros_integration/ros_node_abstraction_iface.hpp"
+  /**
    * @brief Constructor
    */
   MeshDisplay();
@@ -177,6 +177,11 @@ class MeshDisplay : public rviz_common::Display
 
   void fixedFrameChanged() override;
 
+  /**
+   * @brief Used by RViz's "New display by topic" window
+   */
+  void setTopic(const QString& topic, const QString& datatype) override;
+
   /**
    * @brief Update all subscriptions. Individual subscription update function will check whether they are active. (e.g. vertex colors can also be inactive when the UI element is set to a fixed color).
    */

…z_mesh_tools_plugins/ThreadSaveQueue.hpp …z_mesh_tools_plugins/ThreadSafeQueue.hpprviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp renamed to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSaveQueue.hpp renamed to rviz_mesh_tools_plugins/include/rviz_mesh_tools_plugins/ThreadSafeQueue.hpp

@@ -33,6 +33,7 @@
     <description>
       Displays a mesh.
     </description>
+    <message_type>mesh_msgs/msg/MeshGeometryStamped</message_type>
   </class>
   <class name="rviz_mesh_tools_plugins/MeshGoal"
          type="rviz_mesh_tools_plugins::MeshGoalTool"

rviz_mesh_tools_plugins/plugins_description.xml

@@ -284,6 +284,7 @@ void MeshDisplay::onInitialize()
   m_meshTopic->initialize(context_->getRosNodeAbstraction());
   m_vertexColorsTopic->initialize(context_->getRosNodeAbstraction());
   m_vertexCostsTopic->initialize(context_->getRosNodeAbstraction());
+  m_vertexCostUpdateTopic->initialize(context_->getRosNodeAbstraction());
 
   m_meshTopicQos->initialize(
       [this](rclcpp::QoS profile) {
@@ -303,6 +304,12 @@ void MeshDisplay::onInitialize()
         updateVertexCostsSubscription();
       });
 
+  m_vertexCostUpdateTopicQos->initialize(
+      [this](rclcpp::QoS profile) {
+        m_vertexCostUpdateQos = profile;
+        updateVertexCostsUpdateSubscription();
+      });
+
   // Initialize service clients
   //m_vertexColorClient = node->create_client<mesh_msgs::srv::GetVertexColors>(m_vertexColorServiceName->getStdString());
   //m_materialsClient = node->create_client<mesh_msgs::srv::GetMaterials>(m_materialServiceName->getStdString());
@@ -415,6 +422,21 @@ void MeshDisplay::fixedFrameChanged()
   this->transformMesh();
 }
 
+void MeshDisplay::setTopic(const QString& topic, const QString& datatype)
+{
+  (void) datatype;
+  RCLCPP_DEBUG(
+    rclcpp::get_logger("rviz_mesh_tools_plugins"),
+    // The char array returned by QString.data() may not be '\0' terminated! -> topic.toStdString().c_str()
+    "MeshDisplay::setTopic() - called with topic='%s'", topic.toStdString().c_str()
+  );
+  if (m_meshTopic)
+  {
+    // This also triggers the updateMeshGeometrySubscription() slot
+    m_meshTopic->setString(topic);
+  }
+}
+
 void MeshDisplay::reset()
 {
   Display::reset();
@@ -460,6 +482,7 @@ void MeshDisplay::updateAllSubscriptions()
   updateMeshGeometrySubscription();
   updateVertexColorsSubscription();
   updateVertexCostsSubscription();
+  updateVertexCostsUpdateSubscription();
 
   // TODO
   // initialServiceCall();
@@ -470,6 +493,7 @@ void MeshDisplay::unsubscribe()
   m_meshSubscriber.unsubscribe();
   m_vertexColorsSubscriber.unsubscribe();
   m_vertexCostsSubscriber.unsubscribe();
+  m_vertexCostUpdateSubscriber.unsubscribe();
 
   m_tfMeshFilter.reset();
   m_colorsMsgCache.reset();