From b42ad88938151b20b9b907b71cdd055ccb46cbe4 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Sat, 10 Aug 2024 22:30:05 +0300 Subject: [PATCH 1/6] Fixed segmentation fault in depth_image_octomap_updater --- .../src/depth_image_octomap_updater.cpp | 21 ++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) 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..c248ffea28 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,12 @@ 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])); + std::vector tmp_debug_msg_data; + tmp_debug_msg_data.resize(img_size * sizeof(float)); + mesh_filter_->getModelDepth(reinterpret_cast(&tmp_debug_msg_data[0])); + for (int i = 0; i < debug_msg.data.size(); i++) { + debug_msg.data[i] = static_cast(tmp_debug_msg_data[i] * 1000 + 0.5); + } pub_model_depth_image_.publish(debug_msg, *info_msg); sensor_msgs::msg::Image filtered_depth_msg; @@ -449,7 +454,12 @@ 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])); + std::vector tmp_filtered_depth_msg_data; + tmp_filtered_depth_msg_data.resize(img_size * sizeof(float)); + mesh_filter_->getFilteredDepth(reinterpret_cast(&tmp_filtered_depth_msg_data[0])); + for (int i = 0; i < filtered_depth_msg.data.size(); i++) { + filtered_depth_msg.data[i] = static_cast(tmp_filtered_depth_msg_data[i] * 1000 + 0.5); + } pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); sensor_msgs::msg::Image label_msg; @@ -460,7 +470,12 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: label_msg.encoding = sensor_msgs::image_encodings::RGBA8; label_msg.step = w * sizeof(unsigned int); label_msg.data.resize(img_size * sizeof(unsigned int)); - mesh_filter_->getFilteredLabels(reinterpret_cast(&label_msg.data[0])); + std::vector tmp_label_msg_data; + tmp_label_msg_data.resize(img_size * sizeof(unsigned int)); + mesh_filter_->getFilteredLabels(reinterpret_cast(&tmp_label_msg_data[0])); + for (int i = 0; i < filtered_depth_msg.data.size(); i++) { + label_msg.data[i] = static_cast(tmp_label_msg_data[i]); + } pub_filtered_label_image_.publish(label_msg, *info_msg); } From e6115bd6e7f8bd140e9e242575add5900660ddf0 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Sat, 10 Aug 2024 22:47:37 +0300 Subject: [PATCH 2/6] pre-commit fix --- .../src/depth_image_octomap_updater.cpp | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) 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 c248ffea28..7224ecef6d 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 @@ -441,7 +441,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_debug_msg_data; tmp_debug_msg_data.resize(img_size * sizeof(float)); mesh_filter_->getModelDepth(reinterpret_cast(&tmp_debug_msg_data[0])); - for (int i = 0; i < debug_msg.data.size(); i++) { + for (int i = 0; i < debug_msg.data.size(); i++) + { debug_msg.data[i] = static_cast(tmp_debug_msg_data[i] * 1000 + 0.5); } pub_model_depth_image_.publish(debug_msg, *info_msg); @@ -457,7 +458,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_filtered_depth_msg_data; tmp_filtered_depth_msg_data.resize(img_size * sizeof(float)); mesh_filter_->getFilteredDepth(reinterpret_cast(&tmp_filtered_depth_msg_data[0])); - for (int i = 0; i < filtered_depth_msg.data.size(); i++) { + for (int i = 0; i < filtered_depth_msg.data.size(); i++) + { filtered_depth_msg.data[i] = static_cast(tmp_filtered_depth_msg_data[i] * 1000 + 0.5); } pub_filtered_depth_image_.publish(filtered_depth_msg, *info_msg); @@ -473,7 +475,8 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_label_msg_data; tmp_label_msg_data.resize(img_size * sizeof(unsigned int)); mesh_filter_->getFilteredLabels(reinterpret_cast(&tmp_label_msg_data[0])); - for (int i = 0; i < filtered_depth_msg.data.size(); i++) { + for (int i = 0; i < filtered_depth_msg.data.size(); i++) + { label_msg.data[i] = static_cast(tmp_label_msg_data[i]); } From 346326469eb38ed09d6fbab0bc80945e698ab7c4 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Sat, 10 Aug 2024 23:13:33 +0300 Subject: [PATCH 3/6] Convert index type to std::size_t in for loop --- .../src/depth_image_octomap_updater.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 7224ecef6d..9a6a7f7caa 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 @@ -441,7 +441,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_debug_msg_data; tmp_debug_msg_data.resize(img_size * sizeof(float)); mesh_filter_->getModelDepth(reinterpret_cast(&tmp_debug_msg_data[0])); - for (int i = 0; i < debug_msg.data.size(); i++) + for (std::size_t i = 0; i < debug_msg.data.size(); ++i) { debug_msg.data[i] = static_cast(tmp_debug_msg_data[i] * 1000 + 0.5); } @@ -458,7 +458,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_filtered_depth_msg_data; tmp_filtered_depth_msg_data.resize(img_size * sizeof(float)); mesh_filter_->getFilteredDepth(reinterpret_cast(&tmp_filtered_depth_msg_data[0])); - for (int i = 0; i < filtered_depth_msg.data.size(); i++) + for (std::size_t i = 0; i < filtered_depth_msg.data.size(); ++i) { filtered_depth_msg.data[i] = static_cast(tmp_filtered_depth_msg_data[i] * 1000 + 0.5); } @@ -475,7 +475,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_label_msg_data; tmp_label_msg_data.resize(img_size * sizeof(unsigned int)); mesh_filter_->getFilteredLabels(reinterpret_cast(&tmp_label_msg_data[0])); - for (int i = 0; i < filtered_depth_msg.data.size(); i++) + for (std::size_t i = 0; i < filtered_depth_msg.data.size(); ++i) { label_msg.data[i] = static_cast(tmp_label_msg_data[i]); } From 1e6076bf95e5374bb08b48a273f65639b84c86ee Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 12 Aug 2024 10:01:55 +0300 Subject: [PATCH 4/6] Fixed data type problem in depthImageUpdater --- .../src/depth_image_octomap_updater.cpp | 6 +++--- .../mesh_filter/include/moveit/mesh_filter/gl_renderer.h | 2 +- .../include/moveit/mesh_filter/mesh_filter_base.h | 4 ++-- .../mesh_filter/include/moveit/mesh_filter/sensor_model.h | 4 ++-- moveit_ros/perception/mesh_filter/src/gl_renderer.cpp | 2 +- .../perception/mesh_filter/src/mesh_filter_base.cpp | 4 ++-- moveit_ros/perception/mesh_filter/src/sensor_model.cpp | 8 ++++---- 7 files changed, 15 insertions(+), 15 deletions(-) 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) From fedd04c9aae80db8ea2fe2c435cac3d3ef9b7289 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 12 Aug 2024 10:59:46 +0300 Subject: [PATCH 5/6] Remove unnecessary for loop --- .../src/depth_image_octomap_updater.cpp | 4 ---- 1 file changed, 4 deletions(-) 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 ce79dbd92d..b61fd97867 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 @@ -463,10 +463,6 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: std::vector tmp_label_msg_data; tmp_label_msg_data.resize(img_size * sizeof(unsigned int)); mesh_filter_->getFilteredLabels(reinterpret_cast(&tmp_label_msg_data[0])); - for (std::size_t i = 0; i < filtered_depth_msg.data.size(); ++i) - { - label_msg.data[i] = static_cast(tmp_label_msg_data[i]); - } pub_filtered_label_image_.publish(label_msg, *info_msg); } From ed43001639e213383383ec52a06c3899a6cd3fc9 Mon Sep 17 00:00:00 2001 From: CihatAltiparmak Date: Mon, 12 Aug 2024 11:04:44 +0300 Subject: [PATCH 6/6] Remove tmp array --- .../src/depth_image_octomap_updater.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) 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 b61fd97867..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 @@ -460,9 +460,7 @@ void DepthImageOctomapUpdater::depthImageCallback(const sensor_msgs::msg::Image: label_msg.encoding = sensor_msgs::image_encodings::RGBA8; label_msg.step = w * sizeof(unsigned int); label_msg.data.resize(img_size * sizeof(unsigned int)); - std::vector tmp_label_msg_data; - tmp_label_msg_data.resize(img_size * sizeof(unsigned int)); - mesh_filter_->getFilteredLabels(reinterpret_cast(&tmp_label_msg_data[0])); + mesh_filter_->getFilteredLabels(reinterpret_cast(&label_msg.data[0])); pub_filtered_label_image_.publish(label_msg, *info_msg); }