wanngwe/odin_ros_driver1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add online cloud reprojection

Browse Source
This commit is contained in:
hjy
2026-04-08 14:27:30 +08:00
parent 0ec1300ab5
commit 6864f1114c
4 changed files with 63 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/cloud_reprojection_ros.cpp
+34 -4
View File
@@ -147,7 +147,7 @@ void CloudReprojectionRosNode::loadParameters()
combined_jpeg_quality_ = this->get_parameter("register_keys.combined_jpeg_quality").as_int();
combined_jpeg_quality_ = std::max(1, std::min(100, combined_jpeg_quality_));
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
@@ -235,7 +235,6 @@ void CloudReprojectionRosNode::syncCallback(
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_msg)
{
sync_cloud_pub_->publish(*cloud_msg);
sync_odom_pub_->publish(*odom_msg);
sync_wiwc_pub_->publish(*wiwc_msg);
sync_image_pub_->publish(*image_compressed_msg);
@@ -276,6 +275,22 @@ void CloudReprojectionRosNode::syncCallback(
odom_msg->pose.pose.position.z
);
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
if (cloud_cam.empty())
{
RCLCPP_WARN(this->get_logger(), "Camera-frame cloud is empty after reprojection");
return;
}
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
sync_cloud_pub_->publish(cloud_cam_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
@@ -428,7 +443,7 @@ void CloudReprojectionRosNode::loadParameters()
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_slam";
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
@@ -491,7 +506,6 @@ void CloudReprojectionRosNode::syncCallback(
const nav_msgs::OdometryConstPtr& wiwc_msg,
const sensor_msgs::CompressedImageConstPtr& image_compressed_msg)
{
sync_cloud_pub_.publish(cloud_msg);
sync_odom_pub_.publish(odom_msg);
sync_wiwc_pub_.publish(wiwc_msg);
sync_image_pub_.publish(image_compressed_msg);
@@ -531,6 +545,22 @@ void CloudReprojectionRosNode::syncCallback(
odom_msg->pose.pose.position.z
);
pcl::PointCloud<pcl::PointXYZRGB> cloud_cam =
reprojector_->transformCloudToCamera(cloud_odom, odom_pose);
if (cloud_cam.empty())
{
ROS_WARN("Camera-frame cloud is empty after reprojection");
return;
}
sensor_msgs::PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_msg->header;
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
sync_cloud_pub_.publish(cloud_cam_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;