Merge branch 'main' into servo_initial_joints

moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp

@@ -438,7 +438,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     debug_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     debug_msg.step = w * sizeof(float);
     debug_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getModelDepth(reinterpret_cast<double*>(&debug_msg.data[0]));
+    mesh_filter_->getModelDepth(reinterpret_cast<float*>(&debug_msg.data[0]));
     pub_model_depth_image_.publish(debug_msg, *info_msg);
 
     sensor_msgs::msg::Image filtered_depth_msg;
@@ -449,7 +449,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     filtered_depth_msg.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
     filtered_depth_msg.step = w * sizeof(float);
     filtered_depth_msg.data.resize(img_size * sizeof(float));
-    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_depth_msg.data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_depth_msg.data[0]));
     pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg);
 
     sensor_msgs::msg::Image label_msg;
@@ -481,7 +481,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image:
     if (filtered_data.size() < img_size)
       filtered_data.resize(img_size);
 
-    mesh_filter_->getFilteredDepth(reinterpret_cast<double*>(&filtered_data[0]));
+    mesh_filter_->getFilteredDepth(reinterpret_cast<float*>(&filtered_data[0]));
     unsigned short* msg_data = reinterpret_cast<unsigned short*>(&filtered_msg.data[0]);
     for (std::size_t i = 0; i < img_size; ++i)
     {

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h

@@ -106,7 +106,7 @@ class GLRenderer
    * \author Suat Gedikli ([email protected])
    * \param[out] buffer pointer to memory where the depth values need to be stored
    */
-  void getDepthBuffer(double* buffer) const;
+  void getDepthBuffer(float* buffer) const;
 
   /**
    * \brief loads, compiles, links and adds GLSL shaders from files to the current OpenGL context.

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h

@@ -131,7 +131,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getFilteredDepth(double* depth) const;
+  void getFilteredDepth(float* depth) const;
 
   /**
    * \brief retrieves the labels of the rendered model
@@ -149,7 +149,7 @@ class MeshFilterBase
    * \author Suat Gedikli ([email protected])
    * \param[out] depth pointer to buffer to be filled with depth values.
    */
-  void getModelDepth(double* depth) const;
+  void getModelDepth(float* depth) const;
 
   /**
    * \brief set the shadow threshold. points that are further away than the rendered model are filtered out.

moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h

@@ -103,13 +103,13 @@ class SensorModel
      * \brief transforms depth values from rendered model to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformModelDepthToMetricDepth(double* depth) const;
+    virtual void transformModelDepthToMetricDepth(float* depth) const;
 
     /**
      * \brief transforms depth values from filtered depth to metric depth values
      * \param[in,out] depth pointer to floating point depth buffer
      */
-    virtual void transformFilteredDepthToMetricDepth(double* depth) const;
+    virtual void transformFilteredDepthToMetricDepth(float* depth) const;
 
     /**
      * \brief sets  the image size

moveit_ros/perception/mesh_filter/src/gl_renderer.cpp

@@ -213,7 +213,7 @@ void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const
   glBindFramebuffer(GL_FRAMEBUFFER, 0);
 }
 
-void mesh_filter::GLRenderer::getDepthBuffer(double* buffer) const
+void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const
 {
   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
   glBindTexture(GL_TEXTURE_2D, depth_id_);

moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp

@@ -227,7 +227,7 @@ void mesh_filter::MeshFilterBase::getModelLabels(LabelType* labels) const
   job->wait();
 }
 
-void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const
+void mesh_filter::MeshFilterBase::getModelDepth(float* depth) const
 {
   JobPtr job1 =
       std::make_shared<FilterJob<void>>([&renderer = *mesh_renderer_, depth] { renderer.getDepthBuffer(depth); });
@@ -243,7 +243,7 @@ void mesh_filter::MeshFilterBase::getModelDepth(double* depth) const
   job2->wait();
 }
 
-void mesh_filter::MeshFilterBase::getFilteredDepth(double* depth) const
+void mesh_filter::MeshFilterBase::getFilteredDepth(float* depth) const
 {
   JobPtr job1 = std::make_shared<FilterJob<void>>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); });
   JobPtr job2 = std::make_shared<FilterJob<void>>(

moveit_ros/perception/mesh_filter/src/sensor_model.cpp

@@ -102,7 +102,7 @@ inline bool isAligned16(const void* pointer)
 #endif
 }  // namespace
 
-void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(double* depth) const
+void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(float* depth) const
 {
 #if HAVE_SSE_EXTENSIONS
   const __m128 mmNear = _mm_set1_ps(near_clipping_plane_distance_);
@@ -161,7 +161,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub
   const float nf = near * far;
   const float f_n = far - near;
 
-  const double* depth_end = depth + width_ * height_;
+  const float* depth_end = depth + width_ * height_;
   while (depth < depth_end)
   {
     if (*depth != 0 && *depth != 1)
@@ -178,7 +178,7 @@ void mesh_filter::SensorModel::Parameters::transformModelDepthToMetricDepth(doub
 #endif
 }
 
-void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(double* depth) const
+void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(float* depth) const
 {
 #if HAVE_SSE_EXTENSIONS
   //* SSE version
@@ -223,7 +223,7 @@ void mesh_filter::SensorModel::Parameters::transformFilteredDepthToMetricDepth(d
     ++mmDepth;
   }
 #else
-  const double* depth_end = depth + width_ * height_;
+  const float* depth_end = depth + width_ * height_;
   const float scale = far_clipping_plane_distance_ - near_clipping_plane_distance_;
   const float offset = near_clipping_plane_distance_;
   while (depth < depth_end)