diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 5e2fceb8a0..b9cd185d67 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/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(&debug_msg.data[0])); + mesh_filter_->getModelDepth(reinterpret_cast(&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(&filtered_depth_msg.data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&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(&filtered_data[0])); + mesh_filter_->getFilteredDepth(reinterpret_cast(&filtered_data[0])); unsigned short* msg_data = reinterpret_cast(&filtered_msg.data[0]); for (std::size_t i = 0; i < img_size; ++i) { diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h index 3532389869..64b8dadfca 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/gl_renderer.h @@ -106,7 +106,7 @@ class GLRenderer * \author Suat Gedikli (gedikli@willowgarage.com) * \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. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h index 646c3ca569..879a9d87bc 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h +++ b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/mesh_filter_base.h @@ -131,7 +131,7 @@ class MeshFilterBase * \author Suat Gedikli (gedikli@willowgarage.com) * \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 (gedikli@willowgarage.com) * \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. diff --git a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h b/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h index 0ba4405281..607fb663c0 100644 --- a/moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/sensor_model.h +++ b/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 diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index 3c2d7d29e3..0dea00e55c 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/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_); diff --git a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp b/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp index 793058297e..6b4b551b9f 100644 --- a/moveit_ros/perception/mesh_filter/src/mesh_filter_base.cpp +++ b/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>([&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>([&filter = *depth_filter_, depth] { filter.getDepthBuffer(depth); }); JobPtr job2 = std::make_shared>( diff --git a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp b/moveit_ros/perception/mesh_filter/src/sensor_model.cpp index 562e7b4359..9ff5cdbf0c 100644 --- a/moveit_ros/perception/mesh_filter/src/sensor_model.cpp +++ b/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)