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

add overlay image on c++ for debug; unify synced sensor output

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 15:51:26 +08:00
parent 77fac3f793
commit cdfe249111
5 changed files with 109 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/control_command.yaml
+1
View File
@@ -101,6 +101,7 @@ register_keys:
relocalization_map_abs_path: "" # must be set for Relocalization mode or will fail
# To get the mapping result file, please use the set_param.sh script provided: "./set_param.sh save_map 1"
save_map: 0
mapping_result_dest_dir: "" # "": if not specified, save to default location of {ws}/src/odin_ros_driver/map/{driver_start_time}/
mapping_result_file_name: "" # "": if not specified, save to location above with default file name of map_{map_save_time}.bin
include/cloud_reprojection_ros_node.hpp
+4 -4
View File
@@ -60,7 +60,6 @@ private:
std::string wiwc_topic_;
std::string camera_image_topic_;
std::string sync_topic_prefix_;
std::string reprojected_image_topic_;
std::string combined_compressed_topic_;
int combined_jpeg_quality_;
bool publish_combined_compressed_;
@@ -79,6 +78,7 @@ private:
std::string sync_odom_topic_;
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
std::string sync_overlay_image_topic_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_pub_;
rclcpp::Publisher<PointCloud2>::SharedPtr sync_cloud_slam_pub_;
@@ -86,7 +86,7 @@ private:
rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_pub_;
image_transport::Publisher reprojected_image_pub_;
image_transport::Publisher overlay_image_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
std::unique_ptr<CloudReprojector> reprojector_;
@@ -111,7 +111,6 @@ private:
std::string wiwc_topic_;
std::string camera_image_topic_;
std::string sync_topic_prefix_;
std::string reprojected_image_topic_;
std::string combined_compressed_topic_;
int combined_jpeg_quality_;
bool publish_combined_compressed_;
@@ -121,6 +120,7 @@ private:
std::string sync_odom_topic_;
std::string sync_wiwc_topic_;
std::string sync_image_topic_;
std::string sync_overlay_image_topic_;
message_filters::Subscriber<sensor_msgs::PointCloud2> cloud_sub_;
message_filters::Subscriber<nav_msgs::Odometry> odom_sub_;
@@ -138,7 +138,7 @@ private:
ros::Publisher sync_wiwc_pub_;
ros::Publisher sync_image_pub_; // CompressedImage
ros::Publisher reprojected_image_pub_;
ros::Publisher overlay_image_pub_;
ros::Publisher combined_pub_;
std::unique_ptr<CloudReprojector> reprojector_;
script/build_ros.sh
-128
View File
@@ -1,128 +0,0 @@
#!/bin/bash
# Get the directory where the script is located (Odin_ROS_Driver directory)
PKG_DIR="$(cd "$(dirname "$0")/.."; pwd)"
# Calculate the workspace root directory (contains devel, build, src)
WORKSPACE_ROOT="$(dirname "$(dirname "$PKG_DIR")")"
# Workspace source directory (contains all packages)
WORKSPACE_SRC="${WORKSPACE_ROOT}/src"
PROJECT_NAME="odin_ros_driver"
# Define color codes
RED='\033[0;31m'
GREEN='\033[0;32m'
YELLOW='\033[1;33m'
NC='\033[0m'
# Clean workspace function
clean_workspace() {
echo -e "${YELLOW}Cleaning build directories${NC}"
# Clean build artifacts in workspace
rm -rf "${WORKSPACE_ROOT}/build"
rm -rf "${WORKSPACE_ROOT}/install"
rm -rf "${WORKSPACE_ROOT}/log"
rm -rf "${WORKSPACE_ROOT}/devel"
echo -e "${GREEN}Cleanup complete${NC}"
}
# Run node function
run_node() {
echo -e "${YELLOW}Running ROS1 node${NC}"
# Check if environment file exists
if [ ! -f "${WORKSPACE_ROOT}/devel/setup.bash" ]; then
echo -e "${RED}Could not find devel/setup.bash, please build the project with ./build_ros1.sh first${NC}"
return 1
fi
# Source environment and run node
source "${WORKSPACE_ROOT}/devel/setup.bash"
}
# Build workspace function
build_workspace() {
echo -e "${YELLOW}Workspace structure:${NC}"
echo " Workspace root: ${WORKSPACE_ROOT}"
echo " Source directory: ${WORKSPACE_SRC}"
echo " Package directory: ${PKG_DIR}"
echo " ROS version: ROS1"
echo -e "${YELLOW}Starting ROS1 project build...${NC}"
# Clean
cd $WS_DIR
rm -rf build devel install
# Ensure ROS1 environment is loaded
if [ -f "/opt/ros/noetic/setup.bash" ]; then
source "/opt/ros/noetic/setup.bash"
elif [ -f "/opt/ros/melodic/setup.bash" ]; then
source "/opt/ros/melodic/setup.bash"
else
echo -e "${RED}Could not find ROS1 setup.bash file. Please ensure ROS1 is installed.${NC}"
return 1
fi
# Create temporary package.xml
if [ -f "${PKG_DIR}/package_ros1.xml" ]; then
echo "Creating temporary package.xml (using package_ros1.xml)"
cp "${PKG_DIR}/package_ros1.xml" "${PKG_DIR}/package.xml"
TEMP_PACKAGE=true
elif [ -f "${PKG_DIR}/package.xml" ]; then
echo "Using existing package.xml"
else
echo -e "${RED}Could not find package.xml in package directory${NC}"
return 1
fi
# Set build system variable
export BUILD_SYSTEM=ROS1
# Switch to workspace root and build
cd "${WORKSPACE_ROOT}" || return 1
catkin_make -DBUILD_SYSTEM=ROS1 -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -j$(nproc)
BUILD_RESULT=$?
# If build successful, source environment
if [[ $BUILD_RESULT -eq 0 ]]; then
echo -e "${GREEN}ROS1 build successful, loading environment: source devel/setup.bash${NC}"
source "${WORKSPACE_ROOT}/devel/setup.bash"
else
echo -e "${RED}ROS1 build failed, please check error logs${NC}"
fi
}
# Help function
show_help() {
echo -e "${YELLOW}Usage:${NC}"
echo " ./build_ros.sh # Build project"
echo " ./build_ros.sh -c # Clean build artifacts"
echo " ./build_ros.sh -h # Show help information"
echo ""
echo -e "${YELLOW}Current configuration:${NC}"
echo " Project name: ${PROJECT_NAME}"
echo " Package directory: ${PKG_DIR}"
echo " Workspace root: ${WORKSPACE_ROOT}"
echo " Source directory: ${WORKSPACE_SRC}"
}
# Main
case "$1" in
-c|--clean)
clean_workspace
;;
-r|--run)
run_node
;;
-h|--help)
show_help
;;
*)
build_workspace
;;
esac
src/cloud_reprojection_ros.cpp
+103 -23
View File
@@ -51,6 +51,67 @@ cv::Mat decodeCompressedToBgr(const uint8_t* data, size_t len)
return cv::imdecode(raw, cv::IMREAD_COLOR);
}
cv::Mat overlayProjectedCloudOnImage(const cv::Mat& camera_bgr,
const pcl::PointCloud<pcl::PointXYZRGB>& cloud_in_cam,
const CloudReprojector::CameraParams& cam_params,
int point_radius = 2,
float min_depth = 0.5f,
float max_depth = 30.0f)
{
if (camera_bgr.empty()) {
return cv::Mat();
}
cv::Mat overlay = camera_bgr.clone();
mini_vikit::PolynomialCamera camera_model(
cam_params.image_width, cam_params.image_height,
cam_params.A11, cam_params.A22,
cam_params.u0, cam_params.v0,
cam_params.A12,
cam_params.k2, cam_params.k3, cam_params.k4,
cam_params.k5, cam_params.k6, cam_params.k7);
std::vector<cv::Point> pixels;
std::vector<float> depths;
pixels.reserve(cloud_in_cam.size());
depths.reserve(cloud_in_cam.size());
for (const auto& pt : cloud_in_cam) {
if (pt.z <= min_depth || pt.z >= max_depth) {
continue;
}
const Eigen::Vector2d uv = camera_model.world2cam(Eigen::Vector3d(pt.x, pt.y, pt.z));
const int u = static_cast<int>(std::round(uv.x()));
const int v = static_cast<int>(std::round(uv.y()));
if (u < 0 || u >= overlay.cols || v < 0 || v >= overlay.rows) {
continue;
}
pixels.emplace_back(u, v);
depths.emplace_back(static_cast<float>(pt.z));
}
if (pixels.empty()) {
return overlay;
}
cv::Mat depth_values(static_cast<int>(depths.size()), 1, CV_32F, depths.data());
cv::Mat clipped, norm_u8, colors;
cv::min(cv::max(depth_values, min_depth), max_depth, clipped);
clipped = (clipped - min_depth) * (255.0f / (max_depth - min_depth + 1e-6f));
clipped.convertTo(norm_u8, CV_8U);
cv::applyColorMap(norm_u8, colors, cv::COLORMAP_JET);
for (int i = 0; i < colors.rows; ++i) {
const cv::Vec3b color = colors.at<cv::Vec3b>(i, 0);
cv::circle(overlay, pixels[static_cast<size_t>(i)], point_radius,
cv::Scalar(color[0], color[1], color[2]), -1);
}
return overlay;
}
} // namespace
#ifdef ROS2
@@ -101,7 +162,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_* topics under: " << sync_topic_prefix_
<< "\n reprojected_image_topic (depth z grayscale): " << reprojected_image_topic_
<< "\n overlay_img_topic: " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
@@ -121,12 +182,12 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(const rclcpp::NodeOptions& op
sync_wiwc_pub_ = this->create_publisher<Odometry>(sync_wiwc_topic_, 10);
sync_image_pub_ = this->create_publisher<CompressedImage>(sync_image_topic_, 10);
reprojected_image_pub_ = image_transport::create_publisher(this, reprojected_image_topic_);
overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_);
if (publish_combined_compressed_) {
combined_pub_ = this->create_publisher<CompressedImage>(combined_compressed_topic_, 10);
}
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + depth + combined)");
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
@@ -135,7 +196,7 @@ void CloudReprojectionRosNode::loadParameters()
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>("reprojected_image_topic", "/odin1/reprojected_image");
this->declare_parameter<std::string>("overlay_image_topic", "/odin1/overlay_img");
this->declare_parameter<std::string>("register_keys.sync_camera_topic", "/odin1/image/compressed");
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");
@@ -145,7 +206,7 @@ void CloudReprojectionRosNode::loadParameters()
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();
reprojected_image_topic_ = this->get_parameter("reprojected_image_topic").as_string();
// sync_overlay_image_topic_ = this->get_parameter("overlay_image_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();
@@ -159,6 +220,7 @@ void CloudReprojectionRosNode::loadParameters()
sync_odom_topic_ = sync_topic_prefix_ + "/odometry";
sync_wiwc_topic_ = sync_topic_prefix_ + "/wiwc";
sync_image_topic_ = sync_topic_prefix_ + "/image/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
@@ -243,10 +305,20 @@ void CloudReprojectionRosNode::syncCallback(
const Odometry::ConstSharedPtr& wiwc_msg,
const CompressedImage::ConstSharedPtr& image_compressed_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_compressed_msg);
const auto sync_stamp = image_compressed_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;
CompressedImage sync_image_msg = *image_compressed_msg;
sync_image_msg.header.stamp = sync_stamp;
sync_cloud_slam_pub_->publish(sync_cloud_slam_msg);
sync_odom_pub_->publish(sync_odom_msg);
sync_wiwc_pub_->publish(sync_wiwc_msg);
pcl::PointCloud<pcl::PointXYZRGB> cloud_odom;
pcl::fromROSMsg(*cloud_msg, cloud_odom);
@@ -295,28 +367,36 @@ void CloudReprojectionRosNode::syncCallback(
PointCloud2 cloud_cam_msg;
pcl::toROSMsg(cloud_cam, cloud_cam_msg);
cloud_cam_msg.header = image_compressed_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;
}
sync_cloud_pub_->publish(cloud_cam_msg);
sync_image_pub_->publish(sync_image_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
}
auto depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
reprojected_image_pub_.publish(*depth_msg);
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
cam_bgr, cloud_cam, reprojector_->getCameraParams());
if (overlay_vis.empty()) {
return;
}
auto overlay_msg = cv_bridge::CvImage(image_compressed_msg->header, "bgr8", overlay_vis).toImageMsg();
overlay_image_pub_.publish(*overlay_msg);
if (combined_pub_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
RCLCPP_ERROR(this->get_logger(), "cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);
@@ -420,7 +500,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
<< "\n wiwc_topic: " << wiwc_topic_
<< "\n camera_image_topic: " << camera_image_topic_
<< "\n sync_prefix: " << sync_topic_prefix_
<< "\n reprojected_image_topic (depth z): " << reprojected_image_topic_
<< "\n overlay_image_topic (depth z): " << sync_overlay_image_topic_
<< "\n combined_compressed_topic: " << combined_compressed_topic_
<< "\n publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off"));
@@ -438,7 +518,7 @@ CloudReprojectionRosNode::CloudReprojectionRosNode(ros::NodeHandle& nh, ros::Nod
sync_wiwc_pub_ = nh_.advertise<nav_msgs::Odometry>(sync_wiwc_topic_, 1);
sync_image_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(sync_image_topic_, 1);
reprojected_image_pub_ = nh_.advertise<sensor_msgs::Image>(reprojected_image_topic_, 1);
overlay_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_overlay_image_topic_, 1);
if (publish_combined_compressed_) {
combined_pub_ = nh_.advertise<sensor_msgs::CompressedImage>(combined_compressed_topic_, 1);
}
@@ -451,7 +531,7 @@ 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>("reprojected_image_topic", reprojected_image_topic_, std::string("/odin1/reprojected_image"));
pnh_.param<std::string>("overlay_image_topic", sync_overlay_image_topic_, std::string("/odin1/reprojected_image"));
pnh_.param<std::string>("register_keys/sync_camera_topic", camera_image_topic_, std::string("/odin1/image/compressed"));
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"));
@@ -588,7 +668,7 @@ void CloudReprojectionRosNode::syncCallback(
}
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
reprojected_image_pub_.publish(depth_msg);
overlay_image_pub_.publish(depth_msg);
if (publish_combined_compressed_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
src/image_overlay_node.cpp
+1 -1
View File
@@ -39,7 +39,7 @@ cv::Mat resizeToHeight(const cv::Mat& src, int target_h)
ImageOverlayNode::ImageOverlayNode(const rclcpp::NodeOptions& options)
: Node("image_overlay_node", options)
{
this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/reprojected_image");
this->declare_parameter<std::string>("register_keys.overlay_reprojected_topic", "/odin1/overlay_img");
this->declare_parameter<std::string>("register_keys.overlay_camera_topic", "/odin1/image/undistorted");
this->declare_parameter<std::string>("register_keys.overlay_output_topic", "/odin1/combined_image/compressed");
this->declare_parameter<int>("register_keys.overlay_jpeg_quality", 85);