Skip to content

Commit 2134347

Browse files
jian-dongjian-dong
committed
use unique_ptr publish point cloud
1 parent c4852a5 commit 2134347

File tree

5 files changed

+64
-59
lines changed

5 files changed

+64
-59
lines changed

‎orbbec_camera/include/orbbec_camera/ob_camera_node.h‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -415,7 +415,6 @@ class OBCameraNode {
415415
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr depth_cloud_pub_;
416416
bool enable_point_cloud_ = true;
417417
bool enable_colored_point_cloud_ = false;
418-
sensor_msgs::msg::PointCloud2 point_cloud_msg_;
419418
std::recursive_mutex point_cloud_mutex_;
420419

421420
orbbec_camera_msgs::msg::DeviceInfo device_info_;

‎orbbec_camera/include/orbbec_camera/utils.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -34,10 +34,10 @@ sensor_msgs::msg::CameraInfo convertToCameraInfo(OBCameraIntrinsic intrinsic,
3434

3535
void saveRGBPointsToPly(const std::shared_ptr<ob::Frame>& frame, const std::string& fileName);
3636

37-
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2& msg,
37+
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2::UniquePtr& msg,
3838
const std::string& fileName);
3939

40-
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2& msg, const std::string& fileName);
40+
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2::UniquePtr& msg, const std::string& fileName);
4141

4242
void savePointsToPly(const std::shared_ptr<ob::Frame>& frame, const std::string& fileName);
4343

‎orbbec_camera/launch/gemini_330_series.launch.py‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,7 +18,7 @@ def generate_launch_description():
1818
DeclareLaunchArgument('usb_port', default_value=''),
1919
DeclareLaunchArgument('device_num', default_value='1'),
2020
DeclareLaunchArgument('point_cloud_qos', default_value='default'),
21-
DeclareLaunchArgument('enable_point_cloud', default_value='true'),
21+
DeclareLaunchArgument('enable_point_cloud', default_value='false'),
2222
DeclareLaunchArgument('enable_colored_point_cloud', default_value='true'),
2323
DeclareLaunchArgument('connection_delay', default_value='100'),
2424
DeclareLaunchArgument('color_width', default_value='0'),

‎orbbec_camera/src/ob_camera_node.cpp‎

Lines changed: 39 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -1113,16 +1113,17 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
11131113
float v0 = depth_intrinsics.cy * ((float)(height) / depth_intrinsics.height);
11141114

11151115
const auto *depth_data = (uint16_t *)depth_frame->data();
1116-
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
1116+
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
1117+
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
11171118
modifier.setPointCloud2FieldsByString(1, "xyz");
11181119
modifier.resize(width * height);
1119-
point_cloud_msg_.width = depth_frame->width();
1120-
point_cloud_msg_.height = depth_frame->height();
1121-
point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
1122-
point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
1123-
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
1124-
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
1125-
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
1120+
point_cloud_msg->width = depth_frame->width();
1121+
point_cloud_msg->height = depth_frame->height();
1122+
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
1123+
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
1124+
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
1125+
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
1126+
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
11261127
size_t valid_count = 0;
11271128
const static float MIN_DISTANCE = 20.0;
11281129
const static float MAX_DISTANCE = 10000.0;
@@ -1152,17 +1153,17 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
11521153
return;
11531154
}
11541155
if (!ordered_pc_) {
1155-
point_cloud_msg_.is_dense = true;
1156-
point_cloud_msg_.width = valid_count;
1157-
point_cloud_msg_.height = 1;
1156+
point_cloud_msg->is_dense = true;
1157+
point_cloud_msg->width = valid_count;
1158+
point_cloud_msg->height = 1;
11581159
modifier.resize(valid_count);
11591160
}
11601161
auto timestamp = use_hardware_time_ ? fromUsToROSTime(depth_frame->timeStampUs())
11611162
: fromUsToROSTime(depth_frame->systemTimeStampUs());
11621163
std::string frame_id = depth_registration_ ? optical_frame_id_[COLOR] : optical_frame_id_[DEPTH];
1163-
point_cloud_msg_.header.stamp = timestamp;
1164-
point_cloud_msg_.header.frame_id = frame_id;
1165-
depth_cloud_pub_->publish(point_cloud_msg_);
1164+
point_cloud_msg->header.stamp = timestamp;
1165+
point_cloud_msg->header.frame_id = frame_id;
1166+
depth_cloud_pub_->publish(std::move(point_cloud_msg));
11661167

11671168
if (save_point_cloud_) {
11681169
save_point_cloud_ = false;
@@ -1176,7 +1177,7 @@ void OBCameraNode::publishDepthPointCloud(const std::shared_ptr<ob::FrameSet> &f
11761177
}
11771178
RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
11781179
try {
1179-
saveDepthPointsToPly(point_cloud_msg_, filename);
1180+
saveDepthPointsToPly(point_cloud_msg, filename);
11801181
} catch (const std::exception &e) {
11811182
RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what());
11821183
}
@@ -1217,22 +1218,23 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
12171218
float v0 = intrinsics.cy * ((float)(color_height) / intrinsics.height);
12181219
const auto *depth_data = (uint16_t *)depth_frame->data();
12191220
const auto *color_data = (uint8_t *)(rgb_buffer_);
1220-
sensor_msgs::PointCloud2Modifier modifier(point_cloud_msg_);
1221+
auto point_cloud_msg = std::make_unique<sensor_msgs::msg::PointCloud2>();
1222+
sensor_msgs::PointCloud2Modifier modifier(*point_cloud_msg);
12211223
modifier.setPointCloud2FieldsByString(1, "xyz");
1222-
point_cloud_msg_.width = color_frame->width();
1223-
point_cloud_msg_.height = color_frame->height();
1224+
point_cloud_msg->width = color_frame->width();
1225+
point_cloud_msg->height = color_frame->height();
12241226
std::string format_str = "rgb";
1225-
point_cloud_msg_.point_step =
1226-
addPointField(point_cloud_msg_, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
1227-
static_cast<int>(point_cloud_msg_.point_step));
1228-
point_cloud_msg_.row_step = point_cloud_msg_.width * point_cloud_msg_.point_step;
1229-
point_cloud_msg_.data.resize(point_cloud_msg_.height * point_cloud_msg_.row_step);
1230-
sensor_msgs::PointCloud2Iterator<float> iter_x(point_cloud_msg_, "x");
1231-
sensor_msgs::PointCloud2Iterator<float> iter_y(point_cloud_msg_, "y");
1232-
sensor_msgs::PointCloud2Iterator<float> iter_z(point_cloud_msg_, "z");
1233-
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(point_cloud_msg_, "r");
1234-
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(point_cloud_msg_, "g");
1235-
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(point_cloud_msg_, "b");
1227+
point_cloud_msg->point_step =
1228+
addPointField(*point_cloud_msg, format_str, 1, sensor_msgs::msg::PointField::FLOAT32,
1229+
static_cast<int>(point_cloud_msg->point_step));
1230+
point_cloud_msg->row_step = point_cloud_msg->width * point_cloud_msg->point_step;
1231+
point_cloud_msg->data.resize(point_cloud_msg->height * point_cloud_msg->row_step);
1232+
sensor_msgs::PointCloud2Iterator<float> iter_x(*point_cloud_msg, "x");
1233+
sensor_msgs::PointCloud2Iterator<float> iter_y(*point_cloud_msg, "y");
1234+
sensor_msgs::PointCloud2Iterator<float> iter_z(*point_cloud_msg, "z");
1235+
sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*point_cloud_msg, "r");
1236+
sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*point_cloud_msg, "g");
1237+
sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*point_cloud_msg, "b");
12361238
size_t valid_count = 0;
12371239
static const float MIN_DISTANCE = 20.0;
12381240
static const float MAX_DISTANCE = 10000.0;
@@ -1271,16 +1273,16 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
12711273
return;
12721274
}
12731275
if (!ordered_pc_) {
1274-
point_cloud_msg_.is_dense = true;
1275-
point_cloud_msg_.width = valid_count;
1276-
point_cloud_msg_.height = 1;
1276+
point_cloud_msg->is_dense = true;
1277+
point_cloud_msg->width = valid_count;
1278+
point_cloud_msg->height = 1;
12771279
modifier.resize(valid_count);
12781280
}
12791281
auto timestamp = use_hardware_time_ ? fromUsToROSTime(depth_frame->timeStampUs())
12801282
: fromUsToROSTime(depth_frame->systemTimeStampUs());
1281-
point_cloud_msg_.header.stamp = timestamp;
1282-
point_cloud_msg_.header.frame_id = optical_frame_id_[COLOR];
1283-
depth_registration_cloud_pub_->publish(point_cloud_msg_);
1283+
point_cloud_msg->header.stamp = timestamp;
1284+
point_cloud_msg->header.frame_id = optical_frame_id_[COLOR];
1285+
depth_registration_cloud_pub_->publish(std::move(point_cloud_msg));
12841286
if (save_colored_point_cloud_) {
12851287
save_colored_point_cloud_ = false;
12861288
auto now = std::time(nullptr);
@@ -1293,7 +1295,7 @@ void OBCameraNode::publishColoredPointCloud(const std::shared_ptr<ob::FrameSet>
12931295
}
12941296
RCLCPP_INFO_STREAM(logger_, "Saving point cloud to " << filename);
12951297
try {
1296-
saveRGBPointCloudMsgToPly(point_cloud_msg_, filename);
1298+
saveRGBPointCloudMsgToPly(point_cloud_msg, filename);
12971299
} catch (const std::exception &e) {
12981300
RCLCPP_ERROR_STREAM(logger_, "Failed to save point cloud: " << e.what());
12991301
} catch (...) {
@@ -1643,7 +1645,7 @@ void OBCameraNode::onNewFrameCallback(const std::shared_ptr<ob::Frame> &frame,
16431645
image_msg->step = width * unit_step_size_[stream_index];
16441646
image_msg->header.frame_id = frame_id;
16451647
CHECK(image_publishers_.count(stream_index) > 0);
1646-
image_publishers_[stream_index].publish(image_msg);
1648+
image_publishers_[stream_index].publish(std::move(image_msg));
16471649
saveImageToFile(stream_index, image, image_msg);
16481650
}
16491651

‎orbbec_camera/src/utils.cpp‎

Lines changed: 22 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -83,14 +83,15 @@ void saveRGBPointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::stri
8383
}
8484
}
8585

86-
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
86+
void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
8787
const std::string &fileName) {
8888
FILE *fp = fopen(fileName.c_str(), "wb+");
8989
CHECK_NOTNULL(fp);
90+
CHECK_NOTNULL(msg);
9091

91-
sensor_msgs::PointCloud2ConstIterator<float> iter_x(msg, "x");
92-
sensor_msgs::PointCloud2ConstIterator<float> iter_y(msg, "y");
93-
sensor_msgs::PointCloud2ConstIterator<float> iter_z(msg, "z");
92+
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
93+
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
94+
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
9495

9596
// First, count the actual number of valid points
9697
size_t valid_points = 0;
@@ -101,12 +102,12 @@ void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
101102
}
102103

103104
// Reset the iterators
104-
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(msg, "x");
105-
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(msg, "y");
106-
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(msg, "z");
107-
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(msg, "r");
108-
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(msg, "g");
109-
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(msg, "b");
105+
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
106+
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
107+
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
108+
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_r(*msg, "r");
109+
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_g(*msg, "g");
110+
sensor_msgs::PointCloud2ConstIterator<uint8_t> iter_b(*msg, "b");
110111

111112
fprintf(fp, "ply\n");
112113
fprintf(fp, "format ascii 1.0\n");
@@ -130,13 +131,14 @@ void saveRGBPointCloudMsgToPly(const sensor_msgs::msg::PointCloud2 &msg,
130131
fclose(fp);
131132
}
132133

133-
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2 &msg, const std::string &fileName) {
134+
void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2::UniquePtr &msg,
135+
const std::string &fileName) {
134136
FILE *fp = fopen(fileName.c_str(), "wb+");
135137
CHECK_NOTNULL(fp);
136-
137-
sensor_msgs::PointCloud2ConstIterator<float> iter_x(msg, "x");
138-
sensor_msgs::PointCloud2ConstIterator<float> iter_y(msg, "y");
139-
sensor_msgs::PointCloud2ConstIterator<float> iter_z(msg, "z");
138+
CHECK_NOTNULL(msg);
139+
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*msg, "x");
140+
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*msg, "y");
141+
sensor_msgs::PointCloud2ConstIterator<float> iter_z(*msg, "z");
140142

141143
// First, count the actual number of valid points
142144
size_t valid_points = 0;
@@ -147,9 +149,9 @@ void saveDepthPointsToPly(const sensor_msgs::msg::PointCloud2 &msg, const std::s
147149
}
148150

149151
// Reset the iterators
150-
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(msg, "x");
151-
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(msg, "y");
152-
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(msg, "z");
152+
iter_x = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "x");
153+
iter_y = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "y");
154+
iter_z = sensor_msgs::PointCloud2ConstIterator<float>(*msg, "z");
153155

154156
fprintf(fp, "ply\n");
155157
fprintf(fp, "format ascii 1.0\n");
@@ -176,6 +178,8 @@ void savePointsToPly(const std::shared_ptr<ob::Frame> &frame, const std::string
176178
fflush(fp);
177179
fclose(fp);
178180
});
181+
CHECK_NOTNULL(fp);
182+
CHECK_NOTNULL(frame);
179183
fprintf(fp, "ply\n");
180184
fprintf(fp, "format ascii 1.0\n");
181185
fprintf(fp, "element vertex %zu\n", point_size);

0 commit comments

Comments
 (0)