1044 lines
44 KiB
C++
1044 lines
44 KiB
C++
/*
|
|
Copyright 2025 Manifold Tech Ltd.(www.manifoldtech.com.co)
|
|
Licensed under the Apache License, Version 2.0 (the "License");
|
|
you may not use this file except in compliance with the License.
|
|
You may obtain a copy of the License at
|
|
http://www.apache.org/licenses/LICENSE-2.0
|
|
Unless required by applicable law or agreed to in writing, software
|
|
distributed under the License is distributed on an "AS IS" BASIS,
|
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
See the License for the specific language governing permissions and
|
|
limitations under the License.
|
|
*/
|
|
|
|
#include "cloud_reprojection_ros_node.hpp"
|
|
#include "cloud_reprojection_processing.hpp"
|
|
|
|
#include <algorithm>
|
|
#include <chrono>
|
|
#include <filesystem>
|
|
#include <fstream>
|
|
#include <iomanip>
|
|
#include <sstream>
|
|
#include <sys/stat.h>
|
|
#include <thread>
|
|
#include <chrono>
|
|
#include <vector>
|
|
#include <cstdint>
|
|
|
|
#ifdef ROS2
|
|
#include <sensor_msgs/msg/compressed_image.hpp>
|
|
#include <geometry_msgs/msg/point_stamped.hpp>
|
|
#include <std_msgs/msg/float32_multi_array.hpp>
|
|
#include <std_msgs/msg/int32.hpp>
|
|
#include "odin_ros_driver/msg/target_observation.hpp"
|
|
#endif
|
|
|
|
namespace {
|
|
std::filesystem::path get_package_source_directory_from_file()
|
|
{
|
|
return std::filesystem::path(__FILE__).parent_path().parent_path();
|
|
}
|
|
|
|
std::filesystem::path get_target_prediction_root_directory()
|
|
{
|
|
return get_package_source_directory_from_file().parent_path().parent_path().parent_path().parent_path();
|
|
}
|
|
|
|
std::string format_vector3f(const Eigen::Vector3f& value)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << std::fixed << std::setprecision(2)
|
|
<< "[" << value.x() << ", " << value.y() << ", " << value.z() << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_bbox_xyxy(const std::array<float, 4>& bbox)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << std::fixed << std::setprecision(1)
|
|
<< "[" << bbox[0] << ", " << bbox[1] << ", " << bbox[2] << ", " << bbox[3] << "]";
|
|
return oss.str();
|
|
}
|
|
|
|
std::string format_keypoints_xyc(
|
|
const std::array<float, 17 * 3>& keypoints_xyc,
|
|
float min_confidence = 0.5f)
|
|
{
|
|
std::ostringstream oss;
|
|
oss << std::fixed << std::setprecision(2);
|
|
bool first = true;
|
|
for (int i = 0; i < 17; ++i) {
|
|
const float confidence = keypoints_xyc[static_cast<size_t>(i) * 3 + 2];
|
|
if (confidence < min_confidence) {
|
|
continue;
|
|
}
|
|
if (!first) {
|
|
oss << " ";
|
|
}
|
|
first = false;
|
|
oss << i << ":[" << keypoints_xyc[static_cast<size_t>(i) * 3 + 0]
|
|
<< ", " << keypoints_xyc[static_cast<size_t>(i) * 3 + 1]
|
|
<< ", " << confidence << "]";
|
|
}
|
|
if (first) {
|
|
return "none";
|
|
}
|
|
return oss.str();
|
|
}
|
|
} // namespace
|
|
|
|
#ifdef ROS2
|
|
#include <functional>
|
|
#include <yaml-cpp/yaml.h>
|
|
#include <rcpputils/filesystem_helper.hpp>
|
|
#else
|
|
#include <boost/bind.hpp>
|
|
#endif
|
|
|
|
// Fixed Til (T_imu_lidar): lidar position in imu frame, transforms from lidar to imu
|
|
// TODO: Fill in the actual Til values for your sensor setup
|
|
static Eigen::Matrix4d getFixedTil()
|
|
{
|
|
Eigen::Matrix4d Til = Eigen::Matrix4d::Identity();
|
|
Til(0, 3) = -0.02663;
|
|
Til(1, 3) = 0.03447;
|
|
Til(2, 3) = 0.02174;
|
|
return Til;
|
|
}
|
|
|
|
static bool fileExists(const std::string& filename) {
|
|
struct stat buffer;
|
|
return (stat(filename.c_str(), &buffer) == 0);
|
|
}
|
|
|
|
#ifdef ROS2
|
|
static std::string get_package_source_directory() {
|
|
return get_package_source_directory_from_file().string();
|
|
}
|
|
|
|
// ==================== ROS2 Implementation ====================
|
|
|
|
CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& options)
|
|
: Node("cloud_reprojection_node", options)
|
|
{
|
|
loadParameters();
|
|
|
|
RCLCPP_INFO_STREAM(this->get_logger(),
|
|
"\n cloud_slam_topic: " << cloud_slam_topic_
|
|
<< "\n odometry_topic: " << odometry_topic_
|
|
<< "\n wiwc_topic: " << wiwc_topic_
|
|
<< "\n camera_image_topic: " << camera_image_topic_
|
|
<< "\n sync_* topics under: " << sync_topic_prefix_
|
|
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
|
|
<< "\n combined_compressed_topic: " << combined_compressed_topic_
|
|
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
|
|
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off")
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
<< "\n process_target_observation: " << (enable_target_observation_ ? "on" : "off")
|
|
<< "\n debug: " << (debug_target_observation_ ? "on" : "off")
|
|
<< "\n target_observation_topic: " << sync_target_observation_topic_
|
|
<< "\n target_pos_cam_topic: " << sync_target_pos_cam_topic_
|
|
<< "\n target_pos_world_topic: " << sync_target_pos_world_topic_
|
|
#endif
|
|
);
|
|
|
|
cloud_sub_.subscribe(this, cloud_slam_topic_);
|
|
odom_sub_.subscribe(this, odometry_topic_);
|
|
wiwc_sub_.subscribe(this, wiwc_topic_);
|
|
image_sub_.subscribe(this, camera_image_topic_);
|
|
|
|
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
|
|
sync_->registerCallback(std::bind(&CloudReprojectionRosNode::syncCallback, this,
|
|
std::placeholders::_1, std::placeholders::_2, std::placeholders::_3,
|
|
std::placeholders::_4));
|
|
|
|
sync_cloud_pub_ = this->create_publisher<PointCloud2>(sync_cloud_topic_, 10);
|
|
sync_cloud_slam_pub_ = this->create_publisher<PointCloud2>(sync_cloud_slam_topic_, 10);
|
|
sync_odom_pub_ = this->create_publisher<Odometry>(sync_odom_topic_, 10);
|
|
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
|
|
sync_image_pub_ = this->create_publisher<Image>(sync_image_topic_, 10);
|
|
|
|
if (send_overlay_) {
|
|
overlay_compressed_pub_ =
|
|
this->create_publisher<CompressedImage>(sync_overlay_image_topic_, 10);
|
|
}
|
|
if (publish_combined_compressed_) {
|
|
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
|
|
}
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
if (enable_target_observation_) {
|
|
target_observation_pub_ = this->create_publisher<odin_ros_driver::msg::TargetObservation>(
|
|
sync_target_observation_topic_, 10);
|
|
target_pos_cam_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
|
|
sync_target_pos_cam_topic_, 10);
|
|
target_pos_world_pub_ = this->create_publisher<geometry_msgs::msg::PointStamped>(
|
|
sync_target_pos_world_topic_, 10);
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation publishers created successfully | observation=%s | pos_cam=%s | pos_world=%s",
|
|
sync_target_observation_topic_.c_str(),
|
|
sync_target_pos_cam_topic_.c_str(),
|
|
sync_target_pos_world_topic_.c_str());
|
|
}
|
|
#endif
|
|
|
|
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
|
|
}
|
|
|
|
void CloudReprojectionRosNode::loadParameters()
|
|
{
|
|
const auto package_path = get_package_source_directory_from_file();
|
|
const auto target_prediction_root = get_target_prediction_root_directory();
|
|
const auto default_yolo_engine =
|
|
(target_prediction_root / "deploy" / "model" / "yolo26s-pose.trt").string();
|
|
const auto default_yolo_labels =
|
|
(target_prediction_root / "thirdparty" / "YOLOs-CPP-TensorRT" / "models" / "coco.names").string();
|
|
|
|
// Declare and get parameters
|
|
this->declare_parameter<std::string>("cloud_slam_topic", "/odin1/cloud_slam");
|
|
this->declare_parameter<std::string>("odometry_topic", "/odin1/odometry");
|
|
this->declare_parameter<std::string>("wiwc_topic", "/odin1/wiwc");
|
|
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image");
|
|
this->declare_parameter<std::string>("register_keys.sync_topic_prefix", "/odin1/sync");
|
|
this->declare_parameter<std::string>("register_keys.combined_compressed_topic", "/odin1/combined_image/compressed");
|
|
this->declare_parameter<int>("register_keys.combined_jpeg_quality", 85);
|
|
this->declare_parameter<int>("register_keys.send_combined_compressed", 1);
|
|
this->declare_parameter<int>("register_keys.send_overlay", 1);
|
|
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
this->declare_parameter<int>("register_keys.process_target_observation", 1);
|
|
this->declare_parameter<int>("register_keys.debug", 1);
|
|
this->declare_parameter<std::string>("register_keys.target_yolo_engine", default_yolo_engine);
|
|
this->declare_parameter<std::string>("register_keys.target_yolo_labels", default_yolo_labels);
|
|
this->declare_parameter<double>("register_keys.target_yolo_conf", 0.5);
|
|
this->declare_parameter<double>("register_keys.target_yolo_nms", 0.50);
|
|
this->declare_parameter<double>("register_keys.target_min_depth", 0.5);
|
|
this->declare_parameter<double>("register_keys.target_max_depth", 12.0);
|
|
this->declare_parameter<double>("register_keys.target_search_radius_px", 25.0);
|
|
#endif
|
|
|
|
cloud_slam_topic_ = this->get_parameter("cloud_slam_topic").as_string();
|
|
odometry_topic_ = this->get_parameter("odometry_topic").as_string();
|
|
wiwc_topic_ = this->get_parameter("wiwc_topic").as_string();
|
|
camera_image_topic_ = this->get_parameter("register_keys.sync_camera_topic").as_string();
|
|
sync_topic_prefix_ = this->get_parameter("register_keys.sync_topic_prefix").as_string();
|
|
combined_compressed_topic_ = this->get_parameter("register_keys.combined_compressed_topic").as_string();
|
|
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_));
|
|
overlay_jpeg_quality_ = this->get_parameter("register_keys.overlay_jpeg_quality").as_int();
|
|
overlay_jpeg_quality_ = std::max(1, std::min(100, overlay_jpeg_quality_));
|
|
publish_combined_compressed_ =
|
|
(this->get_parameter("register_keys.send_combined_compressed").as_int() != 0);
|
|
send_overlay_ = (this->get_parameter("register_keys.send_overlay").as_int() != 0);
|
|
|
|
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
|
|
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
|
|
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
|
|
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
|
|
sync_image_topic_ = sync_topic_prefix_ + "/image";
|
|
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
|
|
sync_target_observation_topic_ = sync_topic_prefix_ + "/target_observation";
|
|
sync_target_pos_cam_topic_ = sync_topic_prefix_ + "/target_pos_cam";
|
|
sync_target_pos_world_topic_ = sync_topic_prefix_ + "/target_pos_world";
|
|
|
|
// Load camera parameters from calib.yaml file directly
|
|
std::string calib_file = (package_path / "config" / "calib.yaml").string();
|
|
|
|
YAML::Node calib_config;
|
|
try {
|
|
calib_config = YAML::LoadFile(calib_file);
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to load calib.yaml: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
CloudReprojector::CameraParams cam_params;
|
|
try {
|
|
cam_params.image_width = calib_config["cam_0"]["image_width"].as<int>();
|
|
cam_params.image_height = calib_config["cam_0"]["image_height"].as<int>();
|
|
cam_params.A11 = calib_config["cam_0"]["A11"].as<double>();
|
|
cam_params.A12 = calib_config["cam_0"]["A12"].as<double>();
|
|
cam_params.A22 = calib_config["cam_0"]["A22"].as<double>();
|
|
cam_params.u0 = calib_config["cam_0"]["u0"].as<double>();
|
|
cam_params.v0 = calib_config["cam_0"]["v0"].as<double>();
|
|
cam_params.k2 = calib_config["cam_0"]["k2"].as<double>();
|
|
cam_params.k3 = calib_config["cam_0"]["k3"].as<double>();
|
|
cam_params.k4 = calib_config["cam_0"]["k4"].as<double>();
|
|
cam_params.k5 = calib_config["cam_0"]["k5"].as<double>();
|
|
cam_params.k6 = calib_config["cam_0"]["k6"].as<double>();
|
|
cam_params.k7 = calib_config["cam_0"]["k7"].as<double>();
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to parse camera parameters: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
// Load extrinsic parameters
|
|
CloudReprojector::ExtrinsicParams ext_params;
|
|
try {
|
|
auto Tcl_vec = calib_config["Tcl_0"].as<std::vector<double>>();
|
|
|
|
if (Tcl_vec.size() == 16)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
ext_params.Tcl(i, j) = Tcl_vec[i * 4 + j];
|
|
}
|
|
else
|
|
{
|
|
RCLCPP_ERROR(this->get_logger(), "Tcl_0 has invalid size: %zu (expected 16)", Tcl_vec.size());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to parse Tcl_0: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return;
|
|
}
|
|
|
|
ext_params.Til = getFixedTil();
|
|
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
|
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Fixed Til (lidar to imu):\n" << ext_params.Til);
|
|
RCLCPP_INFO_STREAM(this->get_logger(), "Calculated Tic:\n" << ext_params.Tic);
|
|
|
|
RCLCPP_INFO(this->get_logger(), "Camera intrinsics:");
|
|
RCLCPP_INFO(this->get_logger(), "Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
|
RCLCPP_INFO(this->get_logger(), "Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
|
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
|
|
|
reprojector_ = std::make_unique<CloudReprojector>();
|
|
if (!reprojector_->initialize(cam_params, ext_params))
|
|
{
|
|
RCLCPP_ERROR(this->get_logger(), "Failed to initialize CloudReprojector");
|
|
rclcpp::shutdown();
|
|
}
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
enable_target_observation_ =
|
|
(this->get_parameter("register_keys.process_target_observation").as_int() != 0);
|
|
debug_target_observation_ =
|
|
(this->get_parameter("register_keys.debug").as_int() != 0);
|
|
if (enable_target_observation_) {
|
|
try {
|
|
odin_ros_driver::TargetObservationConfig target_config;
|
|
target_config.yolo_engine_path =
|
|
this->get_parameter("register_keys.target_yolo_engine").as_string();
|
|
target_config.yolo_labels_path =
|
|
this->get_parameter("register_keys.target_yolo_labels").as_string();
|
|
target_config.yolo_conf = static_cast<float>(
|
|
this->get_parameter("register_keys.target_yolo_conf").as_double());
|
|
target_config.yolo_nms = static_cast<float>(
|
|
this->get_parameter("register_keys.target_yolo_nms").as_double());
|
|
target_config.min_depth = static_cast<float>(
|
|
this->get_parameter("register_keys.target_min_depth").as_double());
|
|
target_config.max_depth = static_cast<float>(
|
|
this->get_parameter("register_keys.target_max_depth").as_double());
|
|
target_config.search_radius_px = static_cast<float>(
|
|
this->get_parameter("register_keys.target_search_radius_px").as_double());
|
|
target_config.debug = debug_target_observation_;
|
|
target_observation_processor_ =
|
|
std::make_unique<odin_ros_driver::TargetObservationProcessor>();
|
|
target_observation_processor_->initialize(target_config);
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation enabled | engine=%s",
|
|
target_config.yolo_engine_path.c_str());
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation processor initialized successfully | labels=%s",
|
|
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str());
|
|
if (debug_target_observation_) {
|
|
RCLCPP_INFO(
|
|
this->get_logger(),
|
|
"Target observation debug enabled | yolo_engine=%s | yolo_labels=%s | yolo_conf=%.3f | yolo_nms=%.3f | min_depth=%.2f | max_depth=%.2f | search_radius_px=%.1f",
|
|
target_config.yolo_engine_path.c_str(),
|
|
target_config.yolo_labels_path.empty() ? "<default-person>" : target_config.yolo_labels_path.c_str(),
|
|
target_config.yolo_conf,
|
|
target_config.yolo_nms,
|
|
target_config.min_depth,
|
|
target_config.max_depth,
|
|
target_config.search_radius_px);
|
|
}
|
|
} catch (const std::exception& e) {
|
|
enable_target_observation_ = false;
|
|
target_observation_processor_.reset();
|
|
RCLCPP_ERROR(
|
|
this->get_logger(),
|
|
"Failed to initialize target observation processor: %s",
|
|
e.what());
|
|
}
|
|
}
|
|
#endif
|
|
}
|
|
|
|
void CloudReprojectionRosNode::syncCallback(
|
|
const PointCloud2::ConstSharedPtr& cloud_msg,
|
|
const Odometry::ConstSharedPtr& odom_msg,
|
|
const Odometry::ConstSharedPtr& wiwc_msg,
|
|
const Image::ConstSharedPtr& image_msg)
|
|
{
|
|
const auto sync_stamp = image_msg->header.stamp;
|
|
|
|
PointCloud2 sync_cloud_slam_msg = *cloud_msg;
|
|
sync_cloud_slam_msg.header.stamp = sync_stamp;
|
|
Odometry sync_odom_msg = *odom_msg;
|
|
sync_odom_msg.header.stamp = sync_stamp;
|
|
Odometry sync_wiwc_msg = *wiwc_msg;
|
|
sync_wiwc_msg.header.stamp = sync_stamp;
|
|
Image sync_image_msg = *image_msg;
|
|
sync_image_msg.header.stamp = sync_stamp;
|
|
|
|
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
|
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
|
|
|
if (cloud_odom.empty())
|
|
{
|
|
RCLCPP_WARN(this->get_logger(), "Empty cloud_slam received");
|
|
return;
|
|
}
|
|
|
|
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
|
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
|
for (int i = 0; i < 16; ++i) {
|
|
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
|
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
|
}
|
|
|
|
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
|
|
if (T_CL_valid && T_IL_valid) {
|
|
reprojector_->updateExtrinsics(T_CL, T_IL);
|
|
}
|
|
|
|
CloudReprojector::OdomPose odom_pose;
|
|
odom_pose.orientation = Eigen::Quaterniond(
|
|
odom_msg->pose.pose.orientation.w,
|
|
odom_msg->pose.pose.orientation.x,
|
|
odom_msg->pose.pose.orientation.y,
|
|
odom_msg->pose.pose.orientation.z
|
|
);
|
|
odom_pose.position = Eigen::Vector3d(
|
|
odom_msg->pose.pose.position.x,
|
|
odom_msg->pose.pose.position.y,
|
|
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_msg->header;
|
|
cloud_cam_msg.header.stamp = sync_stamp;
|
|
if (cloud_cam_msg.header.frame_id.empty()) {
|
|
// cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
|
|
cloud_cam_msg.header.frame_id = odom_msg->child_frame_id; // odin1_base_link
|
|
}
|
|
|
|
|
|
const bool need_combined = (combined_pub_ != nullptr);
|
|
cv::Mat depth_vis;
|
|
if (need_combined) {
|
|
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
|
|
if (depth_vis.empty()) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
cv::Mat cam_bgr;
|
|
if (send_overlay_ || need_combined
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
|| enable_target_observation_
|
|
#endif
|
|
) {
|
|
try {
|
|
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
|
cam_bgr = cv_ptr->image;
|
|
} catch (const cv_bridge::Exception& e) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv_bridge (sync image): %s", e.what());
|
|
return;
|
|
}
|
|
}
|
|
|
|
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
|
|
sync_odom_pub_->publish(sync_odom_msg);
|
|
sync_wiwc_pub_->publish(sync_wiwc_msg);
|
|
sync_cloud_pub_->publish(cloud_cam_msg);
|
|
sync_image_pub_->publish(sync_image_msg);
|
|
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
odin_ros_driver::TargetObservation target_observation;
|
|
odin_ros_driver::TargetObservationDebugInfo target_debug;
|
|
if (enable_target_observation_ && target_observation_processor_) {
|
|
const auto target_start = std::chrono::steady_clock::now();
|
|
target_observation = target_observation_processor_->process(
|
|
cam_bgr,
|
|
cloud_cam,
|
|
*reprojector_,
|
|
odom_pose,
|
|
debug_target_observation_ ? &target_debug : nullptr);
|
|
const auto target_end = std::chrono::steady_clock::now();
|
|
|
|
if (debug_target_observation_) {
|
|
if (!target_observation.valid) {
|
|
if (target_debug.poses_count == 0) {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(),
|
|
*this->get_clock(),
|
|
2000,
|
|
"Target observation | no detections | yolo=%.2f ms | total=%.2f ms",
|
|
target_debug.yolo_ms,
|
|
target_debug.total_ms);
|
|
} else if (target_debug.tracks_count == 0) {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(),
|
|
*this->get_clock(),
|
|
2000,
|
|
"Target observation | detections=%d tracked=0 current_target_id=%d | yolo=%.2f ms | mot=%.2f ms | total=%.2f ms",
|
|
target_debug.poses_count,
|
|
target_debug.current_target_id_before,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.total_ms);
|
|
} else {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(),
|
|
*this->get_clock(),
|
|
1000,
|
|
"Target observation | detections=%d tracked=%d selected_id=%d det_ind=%d cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms",
|
|
target_debug.poses_count,
|
|
target_debug.tracks_count,
|
|
target_debug.selected_track_id,
|
|
target_debug.detection_index,
|
|
target_debug.projected_cloud_points,
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.depth_ms,
|
|
target_debug.total_ms);
|
|
}
|
|
} else {
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(),
|
|
*this->get_clock(),
|
|
500,
|
|
"Target observation | stamp=%u.%u size=%dx%d cloud=%zu detections=%d tracked=%d current_target_id=%d selected_id=%d det_ind=%d reused=%s bbox=%s depth=%.3f conf=%.3f depth_conf=%.3f pos_cam=%s pos_world=%s cloud_pts=%d depth_samples=%d | yolo=%.2f ms | mot=%.2f ms | depth=%.2f ms | total=%.2f ms | node_total=%.2f ms",
|
|
image_msg->header.stamp.sec,
|
|
image_msg->header.stamp.nanosec,
|
|
cam_bgr.cols,
|
|
cam_bgr.rows,
|
|
cloud_cam.size(),
|
|
target_debug.poses_count,
|
|
target_debug.tracks_count,
|
|
target_debug.current_target_id_before,
|
|
target_observation.track_id,
|
|
target_observation.detection_index,
|
|
target_debug.found_existing_target ? "yes" : "no",
|
|
format_bbox_xyxy(target_observation.bbox_xyxy).c_str(),
|
|
target_observation.depth,
|
|
target_observation.confidence,
|
|
target_observation.depth_confidence,
|
|
format_vector3f(target_observation.target_pos_cam).c_str(),
|
|
format_vector3f(target_observation.target_pos_world).c_str(),
|
|
target_debug.projected_cloud_points,
|
|
target_debug.depth_sample_count,
|
|
target_debug.yolo_ms,
|
|
target_debug.mot_ms,
|
|
target_debug.depth_ms,
|
|
target_debug.total_ms,
|
|
std::chrono::duration<double, std::milli>(target_end - target_start).count());
|
|
RCLCPP_INFO_THROTTLE(
|
|
this->get_logger(),
|
|
*this->get_clock(),
|
|
500,
|
|
"Target observation keypoints: %s",
|
|
format_keypoints_xyc(target_observation.keypoints_xyc).c_str());
|
|
}
|
|
}
|
|
|
|
if (target_observation.valid) {
|
|
odin_ros_driver::msg::TargetObservation observation_msg;
|
|
observation_msg.header = image_msg->header;
|
|
observation_msg.header.stamp = sync_stamp;
|
|
observation_msg.odometry = sync_odom_msg;
|
|
observation_msg.valid = target_observation.valid;
|
|
observation_msg.track_id = target_observation.track_id;
|
|
observation_msg.detection_index = target_observation.detection_index;
|
|
observation_msg.confidence = target_observation.confidence;
|
|
observation_msg.depth = target_observation.depth;
|
|
observation_msg.depth_confidence = target_observation.depth_confidence;
|
|
observation_msg.bbox_xyxy = target_observation.bbox_xyxy;
|
|
observation_msg.keypoints_xyc = target_observation.keypoints_xyc;
|
|
observation_msg.target_pos_cam.x = target_observation.target_pos_cam.x();
|
|
observation_msg.target_pos_cam.y = target_observation.target_pos_cam.y();
|
|
observation_msg.target_pos_cam.z = target_observation.target_pos_cam.z();
|
|
observation_msg.target_pos_world.x = target_observation.target_pos_world.x();
|
|
observation_msg.target_pos_world.y = target_observation.target_pos_world.y();
|
|
observation_msg.target_pos_world.z = target_observation.target_pos_world.z();
|
|
target_observation_pub_->publish(observation_msg);
|
|
|
|
geometry_msgs::msg::PointStamped pos_cam_msg;
|
|
pos_cam_msg.header = image_msg->header;
|
|
pos_cam_msg.header.stamp = sync_stamp;
|
|
pos_cam_msg.header.frame_id = cloud_cam_msg.header.frame_id.empty()
|
|
? "camera"
|
|
: cloud_cam_msg.header.frame_id;
|
|
pos_cam_msg.point.x = target_observation.target_pos_cam.x();
|
|
pos_cam_msg.point.y = target_observation.target_pos_cam.y();
|
|
pos_cam_msg.point.z = target_observation.target_pos_cam.z();
|
|
target_pos_cam_pub_->publish(pos_cam_msg);
|
|
|
|
geometry_msgs::msg::PointStamped pos_world_msg;
|
|
pos_world_msg.header = image_msg->header;
|
|
pos_world_msg.header.stamp = sync_stamp;
|
|
pos_world_msg.header.frame_id = odom_msg->header.frame_id.empty()
|
|
? "odom"
|
|
: odom_msg->header.frame_id;
|
|
pos_world_msg.point.x = target_observation.target_pos_world.x();
|
|
pos_world_msg.point.y = target_observation.target_pos_world.y();
|
|
pos_world_msg.point.z = target_observation.target_pos_world.z();
|
|
target_pos_world_pub_->publish(pos_world_msg);
|
|
}
|
|
}
|
|
#endif
|
|
|
|
if (send_overlay_ && overlay_compressed_pub_) {
|
|
cv::Mat overlay_vis = odin_ros_driver::overlay_projected_cloud_on_image(
|
|
cam_bgr, cloud_cam, reprojector_->getCameraParams());
|
|
#ifdef ODIN_ROS_DRIVER_HAS_TARGET_OBSERVATION
|
|
if (debug_target_observation_ && target_observation_processor_) {
|
|
target_observation_processor_->draw_detected_poses(
|
|
overlay_vis,
|
|
target_debug.poses);
|
|
for (const auto& pixel : target_debug.valid_projected_pixels) {
|
|
if (pixel.x < 0 || pixel.x >= overlay_vis.cols ||
|
|
pixel.y < 0 || pixel.y >= overlay_vis.rows) {
|
|
continue;
|
|
}
|
|
cv::circle(overlay_vis, pixel, 3, cv::Scalar(0, 255, 0), -1);
|
|
}
|
|
}
|
|
if (target_observation.valid && debug_target_observation_) {
|
|
odin_ros_driver::draw_target_observation_overlay(
|
|
overlay_vis,
|
|
target_observation);
|
|
}
|
|
#endif
|
|
if (!overlay_vis.empty()) {
|
|
std::vector<uchar> obuf;
|
|
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
|
|
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)");
|
|
return;
|
|
}
|
|
CompressedImage omsg;
|
|
omsg.header = image_msg->header;
|
|
omsg.format = "jpeg";
|
|
omsg.data.assign(obuf.begin(), obuf.end());
|
|
overlay_compressed_pub_->publish(omsg);
|
|
}
|
|
}
|
|
|
|
if (combined_pub_) {
|
|
const int H = std::max(depth_vis.rows, cam_bgr.rows);
|
|
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
|
|
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
|
|
cv::Mat combined;
|
|
cv::hconcat(left, right, combined);
|
|
|
|
std::vector<uchar> buf;
|
|
const std::vector<int> enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_};
|
|
if (!cv::imencode(".jpg", combined, buf, enc_params)) {
|
|
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed");
|
|
return;
|
|
}
|
|
|
|
CompressedImage out;
|
|
out.header = image_msg->header;
|
|
out.format = "jpeg";
|
|
out.data.assign(buf.begin(), buf.end());
|
|
combined_pub_->publish(out);
|
|
}
|
|
}
|
|
|
|
// ==================== ROS2 Main ====================
|
|
int main(int argc, char** argv)
|
|
{
|
|
rclcpp::init(argc, argv);
|
|
|
|
auto temp_node = std::make_shared<rclcpp::Node>("cloud_reprojection_check");
|
|
|
|
// Check if reprojection is enabled from control_command.yaml
|
|
std::string package_path = get_package_source_directory();
|
|
std::string config_file = package_path + "/config/control_command.yaml";
|
|
|
|
try {
|
|
YAML::Node config = YAML::LoadFile(config_file);
|
|
std::cout << "config: " << config_file << std::endl;
|
|
if (!config["register_keys"] || !config["register_keys"]["sendreprojection"]) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "sendreprojection parameter not found, cloud reprojection disabled.");
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|
|
|
|
int sendreprojection = config["register_keys"]["sendreprojection"].as<int>();
|
|
if (sendreprojection == 0) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Cloud reprojection will not be published.");
|
|
rclcpp::shutdown();
|
|
return 0;
|
|
}
|
|
} catch (const std::exception& e) {
|
|
RCLCPP_ERROR(temp_node->get_logger(), "Failed to read config: %s", e.what());
|
|
rclcpp::shutdown();
|
|
return 1;
|
|
}
|
|
|
|
// Wait for calib.yaml file to be generated by host_sdk_sample
|
|
std::string calib_file = package_path + "/config/calib.yaml";
|
|
RCLCPP_INFO(temp_node->get_logger(), "Waiting for calib.yaml file at: %s", calib_file.c_str());
|
|
|
|
int wait_count = 0;
|
|
while (rclcpp::ok() && !fileExists(calib_file)) {
|
|
if (wait_count % 10 == 0) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Still waiting for calib.yaml file...");
|
|
}
|
|
std::this_thread::sleep_for(std::chrono::milliseconds(5000));
|
|
wait_count++;
|
|
|
|
// Timeout after 5 seconds
|
|
if (wait_count > 10) {
|
|
RCLCPP_ERROR(temp_node->get_logger(), "Timeout waiting for calib.yaml file");
|
|
rclcpp::shutdown();
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
if (!rclcpp::ok()) {
|
|
RCLCPP_INFO(temp_node->get_logger(), "Node shutdown before calib.yaml file was found.");
|
|
return 0;
|
|
}
|
|
|
|
RCLCPP_INFO(temp_node->get_logger(), "Found calib.yaml file! Starting cloud reprojection node...");
|
|
|
|
auto node = std::make_shared<CloudReprojectionRosNode>();
|
|
|
|
RCLCPP_INFO(node->get_logger(), "CloudReprojectionRosNode started");
|
|
|
|
rclcpp::spin(node);
|
|
rclcpp::shutdown();
|
|
|
|
return 0;
|
|
}
|
|
|
|
#else
|
|
// ==================== ROS1 Implementation ====================
|
|
|
|
CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::NodeHandle& pnh)
|
|
: nh_(nh), pnh_(pnh)
|
|
{
|
|
loadParameters();
|
|
|
|
ROS_INFO_STREAM("\n cloud_slam_topic: " << cloud_slam_topic_
|
|
<< "\n odometry_topic: " << odometry_topic_
|
|
<< "\n wiwc_topic: " << wiwc_topic_
|
|
<< "\n camera_image_topic: " << camera_image_topic_
|
|
<< "\n sync_prefix: " << sync_topic_prefix_
|
|
<< "\n overlay_compressed_topic: " << sync_overlay_image_topic_
|
|
<< "\n combined_compressed_topic: " << combined_compressed_topic_
|
|
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
|
|
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
|
|
|
|
cloud_sub_.subscribe(nh_, cloud_slam_topic_, 1);
|
|
odom_sub_.subscribe(nh_, odometry_topic_, 1);
|
|
wiwc_sub_.subscribe(nh_, wiwc_topic_, 1);
|
|
image_sub_.subscribe(nh_, camera_image_topic_, 1);
|
|
|
|
sync_ = std::make_shared<Sync>(MySyncPolicy(10), cloud_sub_, odom_sub_, wiwc_sub_, image_sub_);
|
|
sync_->registerCallback(boost::bind(&CloudReprojectionRosNode::syncCallback, this, _1, _2, _3, _4));
|
|
|
|
sync_cloud_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_topic_, 1);
|
|
sync_cloud_slam_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(sync_cloud_slam_topic_, 1);
|
|
sync_odom_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_odom_topic_, 1);
|
|
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
|
|
sync_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_image_topic_, 1);
|
|
|
|
if (send_overlay_) {
|
|
overlay_compressed_pub_ =
|
|
nh_.advertise<sensor_msgs::CompressedImage>(sync_overlay_image_topic_, 1);
|
|
}
|
|
if (publish_combined_compressed_) {
|
|
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
|
|
}
|
|
|
|
ROS_INFO("CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
|
|
}
|
|
|
|
void CloudReprojectionRosNode::loadParameters()
|
|
{
|
|
pnh_.param<std::string>("cloud_slam_topic", cloud_slam_topic_, std::string("/odin1/cloud_slam"));
|
|
pnh_.param<std::string>("odometry_topic", odometry_topic_, std::string("/odin1/odometry"));
|
|
pnh_.param<std::string>("wiwc_topic", wiwc_topic_, std::string("/odin1/wiwc"));
|
|
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image"));
|
|
pnh_.param<std::string>("register_keys/sync_topic_prefix", sync_topic_prefix_, std::string("/odin1/sync"));
|
|
pnh_.param<std::string>("register_keys/combined_compressed_topic", combined_compressed_topic_, std::string("/odin1/combined_image/compressed"));
|
|
int jq = 85;
|
|
pnh_.param<int>("register_keys/combined_jpeg_quality", jq, 85);
|
|
combined_jpeg_quality_ = std::max(1, std::min(100, jq));
|
|
int ojq = 85;
|
|
pnh_.param<int>("register_keys/overlay_jpeg_quality", ojq, 85);
|
|
overlay_jpeg_quality_ = std::max(1, std::min(100, ojq));
|
|
int send_combined = 1;
|
|
pnh_.param<int>("register_keys/send_combined_compressed", send_combined, 1);
|
|
publish_combined_compressed_ = (send_combined != 0);
|
|
int send_ov = 1;
|
|
pnh_.param<int>("register_keys/send_overlay", send_ov, 1);
|
|
send_overlay_ = (send_ov != 0);
|
|
|
|
sync_cloud_topic_ = sync_topic_prefix_ + "/cloud_in_cam";
|
|
sync_cloud_slam_topic_ = sync_topic_prefix_ + "/cloud_slam";
|
|
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
|
|
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
|
|
sync_image_topic_ = sync_topic_prefix_ + "/image";
|
|
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
|
|
|
|
// Load camera parameters
|
|
CloudReprojector::CameraParams cam_params;
|
|
pnh_.param<int>("cam_0/image_width", cam_params.image_width, 1600);
|
|
pnh_.param<int>("cam_0/image_height", cam_params.image_height, 1296);
|
|
pnh_.param<double>("cam_0/A11", cam_params.A11, 0.0);
|
|
pnh_.param<double>("cam_0/A12", cam_params.A12, 0.0);
|
|
pnh_.param<double>("cam_0/A22", cam_params.A22, 0.0);
|
|
pnh_.param<double>("cam_0/u0", cam_params.u0, 0.0);
|
|
pnh_.param<double>("cam_0/v0", cam_params.v0, 0.0);
|
|
pnh_.param<double>("cam_0/k2", cam_params.k2, 0.0);
|
|
pnh_.param<double>("cam_0/k3", cam_params.k3, 0.0);
|
|
pnh_.param<double>("cam_0/k4", cam_params.k4, 0.0);
|
|
pnh_.param<double>("cam_0/k5", cam_params.k5, 0.0);
|
|
pnh_.param<double>("cam_0/k6", cam_params.k6, 0.0);
|
|
pnh_.param<double>("cam_0/k7", cam_params.k7, 0.0);
|
|
|
|
// Load extrinsic parameters
|
|
CloudReprojector::ExtrinsicParams ext_params;
|
|
std::vector<double> Tcl_vec_param;
|
|
if (pnh_.getParam("Tcl_0", Tcl_vec_param) && Tcl_vec_param.size() == 16)
|
|
{
|
|
for (int i = 0; i < 4; ++i)
|
|
for (int j = 0; j < 4; ++j)
|
|
ext_params.Tcl(i, j) = Tcl_vec_param[i * 4 + j];
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Tcl_0 param missing or invalid.");
|
|
ros::shutdown();
|
|
return;
|
|
}
|
|
|
|
ext_params.Til = getFixedTil();
|
|
ext_params.Tic = CloudReprojector::calculateTic(ext_params.Tcl, ext_params.Til);
|
|
|
|
ROS_INFO_STREAM("Loaded Tcl (camera to lidar):\n" << ext_params.Tcl);
|
|
ROS_INFO_STREAM("Fixed Til (lidar to imu):\n" << ext_params.Til);
|
|
ROS_INFO_STREAM("Calculated Tic:\n" << ext_params.Tic);
|
|
|
|
ROS_INFO("Camera intrinsics:");
|
|
ROS_INFO("Image size: %dx%d", cam_params.image_width, cam_params.image_height);
|
|
ROS_INFO("Intrinsics: A11=%f A12=%f A22=%f u0=%f v0=%f",
|
|
cam_params.A11, cam_params.A12, cam_params.A22, cam_params.u0, cam_params.v0);
|
|
|
|
reprojector_ = std::make_unique<CloudReprojector>();
|
|
if (!reprojector_->initialize(cam_params, ext_params))
|
|
{
|
|
ROS_ERROR("Failed to initialize CloudReprojector");
|
|
ros::shutdown();
|
|
}
|
|
}
|
|
|
|
void CloudReprojectionRosNode::syncCallback(
|
|
const sensor_msgs::PointCloud2ConstPtr& cloud_msg,
|
|
const nav_msgs::OdometryConstPtr& odom_msg,
|
|
const nav_msgs::OdometryConstPtr& wiwc_msg,
|
|
const sensor_msgs::ImageConstPtr& image_msg)
|
|
{
|
|
sync_cloud_slam_pub_.publish(cloud_msg);
|
|
sync_odom_pub_.publish(odom_msg);
|
|
sync_wiwc_pub_.publish(wiwc_msg);
|
|
sync_image_pub_.publish(image_msg);
|
|
|
|
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
|
|
pcl::fromROSMsg(*cloud_msg, cloud_odom);
|
|
|
|
if (cloud_odom.empty())
|
|
{
|
|
ROS_WARN("Empty cloud_slam received");
|
|
return;
|
|
}
|
|
|
|
Eigen::Matrix4d T_CL = Eigen::Matrix4d::Identity();
|
|
Eigen::Matrix4d T_IL = Eigen::Matrix4d::Identity();
|
|
for (int i = 0; i < 16; ++i) {
|
|
T_CL(i / 4, i % 4) = wiwc_msg->pose.covariance[i];
|
|
T_IL(i / 4, i % 4) = wiwc_msg->twist.covariance[i];
|
|
}
|
|
|
|
bool T_CL_valid = (T_CL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
bool T_IL_valid = (T_IL - Eigen::Matrix4d::Identity()).norm() > 1e-6;
|
|
if (T_CL_valid && T_IL_valid) {
|
|
reprojector_->updateExtrinsics(T_CL, T_IL);
|
|
}
|
|
|
|
CloudReprojector::OdomPose odom_pose;
|
|
odom_pose.orientation = Eigen::Quaterniond(
|
|
odom_msg->pose.pose.orientation.w,
|
|
odom_msg->pose.pose.orientation.x,
|
|
odom_msg->pose.pose.orientation.y,
|
|
odom_msg->pose.pose.orientation.z
|
|
);
|
|
odom_pose.position = Eigen::Vector3d(
|
|
odom_msg->pose.pose.position.x,
|
|
odom_msg->pose.pose.position.y,
|
|
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_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);
|
|
|
|
const bool need_combined = publish_combined_compressed_;
|
|
cv::Mat depth_vis;
|
|
if (need_combined) {
|
|
depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
|
|
if (depth_vis.empty()) {
|
|
return;
|
|
}
|
|
}
|
|
|
|
cv::Mat cam_bgr;
|
|
if (send_overlay_ || need_combined) {
|
|
try {
|
|
cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg, "bgr8");
|
|
cam_bgr = cv_ptr->image;
|
|
} catch (const cv_bridge::Exception& e) {
|
|
ROS_ERROR("cv_bridge (sync image): %s", e.what());
|
|
return;
|
|
}
|
|
}
|
|
|
|
if (send_overlay_) {
|
|
cv::Mat overlay_vis =
|
|
odin_ros_driver::overlay_projected_cloud_on_image(cam_bgr, cloud_cam, reprojector_->getCameraParams());
|
|
if (!overlay_vis.empty()) {
|
|
std::vector<uchar> obuf;
|
|
const std::vector<int> oenc = {cv::IMWRITE_JPEG_QUALITY, overlay_jpeg_quality_};
|
|
if (!cv::imencode(".jpg", overlay_vis, obuf, oenc)) {
|
|
ROS_ERROR("cv::imencode failed (overlay)");
|
|
return;
|
|
}
|
|
sensor_msgs::CompressedImage omsg;
|
|
omsg.header = image_msg->header;
|
|
omsg.format = "jpeg";
|
|
omsg.data.assign(obuf.begin(), obuf.end());
|
|
overlay_compressed_pub_.publish(omsg);
|
|
}
|
|
}
|
|
|
|
if (publish_combined_compressed_) {
|
|
const int H = std::max(depth_vis.rows, cam_bgr.rows);
|
|
cv::Mat left = odin_ros_driver::resize_to_height(depth_vis, H);
|
|
cv::Mat right = odin_ros_driver::resize_to_height(cam_bgr, H);
|
|
cv::Mat combined;
|
|
cv::hconcat(left, right, combined);
|
|
|
|
std::vector<uchar> buf;
|
|
const std::vector<int> enc_params = {cv::IMWRITE_JPEG_QUALITY, combined_jpeg_quality_};
|
|
if (!cv::imencode(".jpg", combined, buf, enc_params)) {
|
|
ROS_ERROR("cv::imencode failed");
|
|
return;
|
|
}
|
|
|
|
sensor_msgs::CompressedImage out;
|
|
out.header = image_msg->header;
|
|
out.format = "jpeg";
|
|
out.data.assign(buf.begin(), buf.end());
|
|
combined_pub_.publish(out);
|
|
}
|
|
}
|
|
|
|
// ==================== ROS1 Main ====================
|
|
int main(int argc, char **argv)
|
|
{
|
|
ros::init(argc, argv, "cloud_reprojection");
|
|
ros::NodeHandle nh;
|
|
ros::NodeHandle pnh("~");
|
|
|
|
// Check if reprojection is enabled
|
|
int sendreprojection = 0;
|
|
pnh.param("register_keys/sendreprojection", sendreprojection, 0);
|
|
if(sendreprojection == 0)
|
|
{
|
|
ROS_INFO("Cloud reprojection will not be published.");
|
|
return 0;
|
|
}
|
|
|
|
std::string calib_file_path;
|
|
pnh.param<std::string>("calib_file_path", calib_file_path, "");
|
|
|
|
ROS_INFO("Waiting for calib.yaml file at: %s", calib_file_path.c_str());
|
|
while(ros::ok() && !fileExists(calib_file_path))
|
|
{
|
|
ROS_INFO_THROTTLE(5, "Still waiting for calib.yaml file...");
|
|
ros::Duration(0.5).sleep();
|
|
ros::spinOnce();
|
|
}
|
|
|
|
if(!ros::ok())
|
|
{
|
|
ROS_INFO("Node shutdown before calib.yaml file was found.");
|
|
return 0;
|
|
}
|
|
|
|
ROS_INFO("Found calib.yaml file! Loading parameters...");
|
|
|
|
std::string node_name = ros::this_node::getName();
|
|
std::string rosparam_command = "rosparam load " + calib_file_path + " " + node_name;
|
|
int result = system(rosparam_command.c_str());
|
|
|
|
if(result == 0)
|
|
{
|
|
ROS_INFO("Successfully loaded parameters from calib.yaml to namespace: %s", node_name.c_str());
|
|
}
|
|
else
|
|
{
|
|
ROS_ERROR("Failed to load parameters from calib.yaml");
|
|
return 1;
|
|
}
|
|
|
|
CloudReprojectionRosNode reprojection_node(nh, pnh);
|
|
ros::spin();
|
|
return 0;
|
|
}
|
|
#endif
|