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

change into compressed image to publish overlay

Browse Source
This commit is contained in:
jingyang-huang
2026-04-09 15:51:52 +08:00
parent cdfe249111
commit 78b8d371ea
5 changed files with 237 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
-1
View File
@@ -341,7 +341,6 @@ elseif(ROS_VERSION STREQUAL "ROS2")
sensor_msgs
nav_msgs
cv_bridge
image_transport
pcl_conversions
message_filters
)
config/control_command.yaml
+3
View File
@@ -79,6 +79,9 @@ register_keys:
combined_jpeg_quality: 85
# 0: 不发布 combined 拼接 JPEG，不创建该 publisher（省 CPU/带宽）；1: 发布 depth|camera 拼接图
send_combined_compressed: 0
# 0: 不发布 overlay（不创建 publisher）；1: 发布 sensor_msgs/CompressedImage JPEG，话题 {sync_topic_prefix}/overlay_img/compressed
send_overlay: 1
overlay_jpeg_quality: 85
# record rgb, odometry, and slam cloud data as proprietary olx format for further processing in MindCloud(TM) software.
# save path: ws/src/odin_ros_driver/recorddata/{record_start_time}/
include/cloud_reprojection_ros_node.hpp
+6 -5
View File
@@ -16,10 +16,8 @@ limitations under the License.
#ifdef ROS2
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/compressed_image.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <image_transport/image_transport.hpp>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
@@ -52,7 +50,6 @@ public:
private:
using PointCloud2 = sensor_msgs::msg::PointCloud2;
using Odometry = nav_msgs::msg::Odometry;
using Image = sensor_msgs::msg::Image;
using CompressedImage = sensor_msgs::msg::CompressedImage;
std::string cloud_slam_topic_;
@@ -62,7 +59,9 @@ private:
std::string sync_topic_prefix_;
std::string combined_compressed_topic_;
int combined_jpeg_quality_;
int overlay_jpeg_quality_;
bool publish_combined_compressed_;
bool send_overlay_;
message_filters::Subscriber<PointCloud2> cloud_sub_;
message_filters::Subscriber<Odometry> odom_sub_;
@@ -86,7 +85,7 @@ private:
rclcpp::Publisher<Odometry>::SharedPtr sync_wiwc_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr sync_image_pub_;
image_transport::Publisher overlay_image_pub_;
rclcpp::Publisher<CompressedImage>::SharedPtr overlay_compressed_pub_; // optional if send_overlay_
rclcpp::Publisher<CompressedImage>::SharedPtr combined_pub_; // optional if publish_combined_compressed_
std::unique_ptr<CloudReprojector> reprojector_;
@@ -113,7 +112,9 @@ private:
std::string sync_topic_prefix_;
std::string combined_compressed_topic_;
int combined_jpeg_quality_;
int overlay_jpeg_quality_;
bool publish_combined_compressed_;
bool send_overlay_;
std::string sync_cloud_topic_;
std::string sync_cloud_slam_topic_;
@@ -138,7 +139,7 @@ private:
ros::Publisher sync_wiwc_pub_;
ros::Publisher sync_image_pub_; // CompressedImage
ros::Publisher overlay_image_pub_;
ros::Publisher overlay_compressed_pub_;
ros::Publisher combined_pub_;
std::unique_ptr<CloudReprojector> reprojector_;
script/ros1_build_ros.sh
Executable
+128
View File
@@ -0,0 +1,128 @@
#!/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
+100 -42
View File
@@ -162,9 +162,10 @@ 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 overlay_img_topic: " << sync_overlay_image_topic_
<< "\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 publish_combined_compressed: " << (publish_combined_compressed_ ? "on" : "off")
<< "\n send_overlay: " << (send_overlay_ ? "on" : "off"));
cloud_sub_.subscribe(this, cloud_slam_topic_);
odom_sub_.subscribe(this, odometry_topic_);
@@ -182,12 +183,15 @@ 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);
overlay_image_pub_ = image_transport::create_publisher(this, sync_overlay_image_topic_);
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);
}
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + overlay + combined)");
RCLCPP_INFO(this->get_logger(), "CloudReprojectionRosNode initialized (4-way sync + optional overlay + combined)");
}
void CloudReprojectionRosNode::loadParameters()
@@ -196,31 +200,34 @@ 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>("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");
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);
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();
// 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();
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/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters from calib.yaml file directly
std::string package_path = get_package_source_directory();
@@ -316,9 +323,7 @@ void CloudReprojectionRosNode::syncCallback(
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);
@@ -371,31 +376,52 @@ void CloudReprojectionRosNode::syncCallback(
if (cloud_cam_msg.header.frame_id.empty()) {
cloud_cam_msg.header.frame_id = cloud_msg->header.frame_id;
}
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) {
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;
}
}
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);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
if (send_overlay_ && overlay_compressed_pub_) {
cv::Mat overlay_vis = overlayProjectedCloudOnImage(
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)) {
RCLCPP_ERROR(this->get_logger(), "cv::imencode failed (overlay)");
return;
}
CompressedImage omsg;
omsg.header = image_compressed_msg->header;
omsg.format = "jpeg";
omsg.data.assign(obuf.begin(), obuf.end());
overlay_compressed_pub_->publish(omsg);
}
}
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_) {
const int H = std::max(depth_vis.rows, cam_bgr.rows);
cv::Mat left = resizeToHeight(depth_vis, H);
@@ -500,9 +526,10 @@ 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 overlay_image_topic (depth z): " << sync_overlay_image_topic_
<< "\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 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);
@@ -518,7 +545,10 @@ 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);
overlay_image_pub_ = nh_.advertise<sensor_msgs::Image>(sync_overlay_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);
}
@@ -531,22 +561,28 @@ 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>("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"));
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/compressed";
sync_overlay_image_topic_ = sync_topic_prefix_ + "/overlay_img/compressed";
// Load camera parameters
CloudReprojector::CameraParams cam_params;
@@ -662,23 +698,45 @@ void CloudReprojectionRosNode::syncCallback(
}
sync_cloud_pub_.publish(cloud_cam_msg);
cv::Mat depth_vis = reprojector_->reprojectCloudDepth(cloud_odom, odom_pose);
if (depth_vis.empty()) {
return;
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;
}
}
sensor_msgs::ImagePtr depth_msg = cv_bridge::CvImage(cloud_msg->header, "bgr8", depth_vis).toImageMsg();
overlay_image_pub_.publish(depth_msg);
if (publish_combined_compressed_) {
cv::Mat cam_bgr = decodeCompressedToBgr(
cv::Mat cam_bgr;
if (send_overlay_ || need_combined) {
cam_bgr = decodeCompressedToBgr(
image_compressed_msg->data.data(), image_compressed_msg->data.size());
if (cam_bgr.empty()) {
ROS_ERROR("cv::imdecode failed (sync compressed image, format=%s)",
image_compressed_msg->format.c_str());
return;
}
}
if (send_overlay_) {
cv::Mat overlay_vis =
overlayProjectedCloudOnImage(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_compressed_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 = resizeToHeight(depth_vis, H);
cv::Mat right = resizeToHeight(cam_bgr, H);