added invalid_depth_ parameter

.gitignore

@@ -1,3 +1,3 @@
 *pyc
-.vscode/settings.json
+.vscode/
 */doc/generated

depth_image_proc/include/depth_image_proc/conversions.hpp

@@ -53,7 +53,7 @@ void convertDepth(
  const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg,
  sensor_msgs::msg::PointCloud2::SharedPtr & cloud_msg,
  const image_geometry::PinholeCameraModel & model,
- double range_max = 0.0)
+ double default_depth = 0.0)
 {
  // Use correct principal point from calibration
  float center_x = model.cx();
@@ -64,20 +64,28 @@ void convertDepth(
  float constant_x = unit_scaling / model.fx();
  float constant_y = unit_scaling / model.fy();
  float bad_point = std::numeric_limits<float>::quiet_NaN();
-
+
+ // the following lines ensure that the computation only happens in case we have a default depth
+ const T default_depth_cvt;
+ bool use_default_depth = default_depth != 0.0;
+ if (use_default_depth) {
+ default_depth_cvt=DepthTraits<T>::fromMeters(default_depth);
+ }
  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
+
+ // TODO: i think this is undefined behaviour we should favor memcpy? https://stackoverflow.com/questions/55150001/vector-with-reinterpret-cast
  const T * depth_row = reinterpret_cast<const T *>(&depth_msg->data[0]);
- int row_step = depth_msg->step / sizeof(T);
- for (int v = 0; v < static_cast<int>(cloud_msg->height); ++v, depth_row += row_step) {
- for (int u = 0; u < static_cast<int>(cloud_msg->width); ++u, ++iter_x, ++iter_y, ++iter_z) {
+ uint32_t row_step = depth_msg->step / sizeof(T);
+ for (uint32_t v = 0; v < cloud_msg->height; ++v, depth_row += row_step) {
+ for (uint32_t u = 0; u < cloud_msg->width; ++u, ++iter_x, ++iter_y, ++iter_z) {
  T depth = depth_row[u];
 
  // Missing points denoted by NaNs
  if (!DepthTraits<T>::valid(depth)) {
- if (range_max != 0.0) {
- depth = DepthTraits<T>::fromMeters(range_max);
+ if (use_default_depth) {
+ depth = default_depth_cvt;
  } else {
  *iter_x = *iter_y = *iter_z = bad_point;
  continue;

depth_image_proc/include/depth_image_proc/point_cloud_xyz.hpp

@@ -67,6 +67,9 @@ class PointCloudXyzNode : public rclcpp::Node
  image_transport::CameraSubscriber sub_depth_;
  int queue_size_;
 
+ // Parameters
+ double invalid_depth_;
+
  // Publications
  std::mutex connect_mutex_;
  rclcpp::Publisher<PointCloud2>::SharedPtr pub_point_cloud_;

depth_image_proc/src/point_cloud_xyz.cpp

@@ -58,6 +58,9 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options)
  // Read parameters
  queue_size_ = this->declare_parameter<int>("queue_size", 5);
 
+ // values used for invalid points for pcd conversion
+ invalid_depth_ = this->declare_parameter<double>("invalid_depth_", 0.0);
+
  // Create publisher with connect callback
  rclcpp::PublisherOptions pub_options;
  pub_options.event_callbacks.matched_callback =
@@ -109,9 +112,9 @@ void PointCloudXyzNode::depthCb(
 
  // Convert Depth Image to Pointcloud
  if (depth_msg->encoding == enc::TYPE_16UC1 || depth_msg->encoding == enc::MONO16) {
- convertDepth<uint16_t>(depth_msg, cloud_msg, model_);
+ convertDepth<uint16_t>(depth_msg, cloud_msg, model_, invalid_depth_);
  } else if (depth_msg->encoding == enc::TYPE_32FC1) {
- convertDepth<float>(depth_msg, cloud_msg, model_);
+ convertDepth<float>(depth_msg, cloud_msg, model_, invalid_depth_);
  } else {
  RCLCPP_ERROR(
  get_logger(), "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());