File tree 1 file changed +9
-2
lines changed
perception/autoware_image_projection_based_fusion/src/segmentation_pointcloud_fusion
1 file changed +9
-2
lines changed Original file line number Diff line number Diff line change @@ -152,6 +152,7 @@ void SegmentPointCloudFusionNode::postprocess(
152
152
std::unique_ptr<ScopedTimeTrack> st_ptr;
153
153
if (time_keeper_) st_ptr = std::make_unique<ScopedTimeTrack>(__func__, *time_keeper_);
154
154
155
+ output_msg = pointcloud_msg;
155
156
output_msg.header = pointcloud_msg.header ;
156
157
output_msg.data .clear ();
157
158
output_msg.data .resize (pointcloud_msg.data .size ());
@@ -166,8 +167,14 @@ void SegmentPointCloudFusionNode::postprocess(
166
167
}
167
168
168
169
output_msg.data .resize (output_pointcloud_size);
169
- output_msg.row_step = output_pointcloud_size / output_msg.height ;
170
- output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height ;
170
+ if (output_msg.height != 0 && output_msg.point_step != 0 ) {
171
+ output_msg.row_step = output_pointcloud_size / output_msg.height ;
172
+ output_msg.width = output_pointcloud_size / output_msg.point_step / output_msg.height ;
173
+ } else {
174
+ RCLCPP_ERROR (this ->get_logger (), " output_msg.height or output_msg.point_step is 0" );
175
+ output_msg.row_step = 0 ;
176
+ output_msg.width = 0 ;
177
+ }
171
178
172
179
filter_global_offset_set_.clear ();
173
180
return ;
You can’t perform that action at this time.
0 commit comments