Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Support 2 res for LPC #2952

Merged
merged 3 commits into from
Dec 25, 2023
Merged

Support 2 res for LPC #2952

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9b5369f
support 2 res for LPC
Nir-Az Dec 21, 2023
1160f2d
get LPC w & h from API
Nir-Az Dec 24, 2023
b929cb3
use new LPC API get_bits_per_pixel
Nir-Az Dec 25, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion realsense2_camera/include/base_realsense_node.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,7 @@ namespace realsense2_camera
void startDynamicTf();
void publishDynamicTransforms();
void publishPointCloud(rs2::points f, const rclcpp::Time& t, const rs2::frameset& frameset);
void publishLabeledPointCloud(rs2::labeled_points points, const rclcpp::Time& t);
void publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t);
bool shouldPublishCameraInfo(const stream_index_pair& sip);
Extrinsics rsExtrinsicsToMsg(const rs2_extrinsics& extrinsics) const;
IMUInfo getImuInfo(const rs2::stream_profile& profile);
Expand Down
17 changes: 9 additions & 8 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -841,7 +841,7 @@ bool BaseRealSenseNode::shouldPublishCameraInfo(const stream_index_pair& sip)
return (stream != RS2_STREAM_SAFETY && stream != RS2_STREAM_OCCUPANCY && stream != RS2_STREAM_LABELED_POINT_CLOUD);
}

void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const rclcpp::Time& t){
void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points lpc, const rclcpp::Time& t){
if(!_labeled_pointcloud_publisher || 0 == _labeled_pointcloud_publisher->get_subscription_count())
return;

Expand All @@ -857,22 +857,23 @@ void BaseRealSenseNode::publishLabeledPointCloud(rs2::labeled_points pc, const r
"y", 1, sensor_msgs::msg::PointField::FLOAT32,
"z", 1, sensor_msgs::msg::PointField::FLOAT32,
"label", 1, sensor_msgs::msg::PointField::UINT8);
modifier.resize(pc.size());
modifier.resize(lpc.size());

// Fill the PointCloud message with data
sensor_msgs::PointCloud2Iterator<float> iter_x(*msg_pointcloud, "x");
sensor_msgs::PointCloud2Iterator<float> iter_y(*msg_pointcloud, "y");
sensor_msgs::PointCloud2Iterator<float> iter_z(*msg_pointcloud, "z");
sensor_msgs::PointCloud2Iterator<uint8_t> iter_label(*msg_pointcloud, "label");
const rs2::vertex* vertex = pc.get_vertices();
const uint8_t* label = pc.get_labels();
msg_pointcloud->width = 320;
msg_pointcloud->height = 180;
msg_pointcloud->point_step = 3*sizeof(float) + sizeof(uint8_t);
const rs2::vertex* vertex = lpc.get_vertices();
const uint8_t* label = lpc.get_labels();

msg_pointcloud->width = lpc.get_width();
msg_pointcloud->height = lpc.get_height();
msg_pointcloud->point_step = lpc.get_bits_per_pixel() / 8;
msg_pointcloud->row_step = msg_pointcloud->width * msg_pointcloud->point_step;
msg_pointcloud->data.resize(msg_pointcloud->height * msg_pointcloud->row_step);

for (size_t point_idx=0; point_idx < pc.size(); point_idx++, vertex++, label++)
for (size_t point_idx=0; point_idx < lpc.size(); point_idx++, vertex++, label++)
{
*iter_x = vertex->x;
*iter_y = vertex->y;
Expand Down