节点1发布位姿，节点2接收位姿，返回图像和位姿，节点1获取数据暂存队列，并单独开线程处理数据
运行脚本
#!/bin/bash #外部给与执行权限 #sudo chmod +x run_ros_nodes.sh # 定义 ROS 安装路径 #安装时候添加到系统路径了 不需要每次都source ROS_SETUP="/opt/ros/noetic/setup.bash" # 定义工作目录路径 自己的工程没有加到系统路径，每次需要source WORKSPACE_DIR="/home/dongdong/2project/1salm/2Mycode/ros/ros_cgg" #指定目录 # 启动 ROS Master echo "Starting ROS 总结点..." gnome-terminal -- bash -c "\ cd $WORKSPACE_DIR; source devel/setup.bash; \ roscore; \ exec bash" # 等待 ROS Master 启动 sleep 5 # 运行 C++ 发布节点 echo "Running C++ 发布节点..." gnome-terminal -- bash -c "\ cd $WORKSPACE_DIR; source devel/setup.bash; \ rosrun image_gaosi image_pose_publisher; \ exec bash" # 运行 C++ 接受节点 python_DIR="/home/dongdong/2project/1salm/2Mycode/ros/ros_cgg/src/image_gaosi/src" echo "Running python 订阅节点..." gnome-terminal -- bash -c "\ cd $python_DIR; \ /usr/bin/python3 image_gps_subscriber.py; \ exec bash"
编译
#=============文件结构 ros_cgg/ ├── build ├── devel ├── src │ ├── CMakeLists.txt -> /opt/ros/noetic/share/catkin/cmake/toplevel.cmake │ ├── image_gaosi │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── src │ │ ├── image_gps_subscriber.py │ │ └── image_pose_publisher.cpp │ └── image_gps_package │ ├── CMakeLists.txt │ ├── package.xml │ └── src │ ├── image_gps_publisher.cpp │ ├── image_gps_publisher（复件）.cpp │ └── image_gps_subscriber.cpp ├── v1_run_ros_nodes.sh └── v2_run_ros_nodes.sh #==============编译 cd ~/ros_cgg catkin_make source devel/setup.bash #==============手动运行 #0开启source确保找到 cd ~/ros_cgg source devel/setup.bash # 手动开启三个窗口 #1启动ROS Master： roscore #2运行你的C++发布节点： rosrun image_gaosi image_pose_publisher #3运行python接受节点 python3 image_gps_subscriber.py
源文件
创建 image_gaosi 工程节点
CMakeLists.txt
注意： 只需要编译c++写的节点image_pose_publisher，python编写的ros 节点不需要任何编译，直接运行就可以跑。
cmake_minimum_required(VERSION 3.0.2)
project(image_gaosi)
find_package(catkin REQUIRED COMPONENTS
roscpp
geometry_msgs
sensor_msgs
cv_bridge
message_filters # 消息同步
image_transport
)
find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS filesystem)
find_package(Eigen3 REQUIRED)
catkin_package(
CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs cv_bridge
DEPENDS Boost
)
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS}
"/usr/local/include/eigen3"
)
# 编译发布节点
add_executable(image_pose_publisher src/image_pose_publisher.cpp)
target_link_libraries(image_pose_publisher
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
${Boost_LIBRARIES}
)
package.xml
注意 <name>image_gaosi</name> 定义了包的名字，rosrun 包名字 节点名字
<?xml version="1.0"?>
<package format="2">
<name>image_gaosi</name>
<version>0.0.1</version>
<description>
A package to publish and subscribe to images and GPS data using ROS.
</description>
<!-- Maintainer of the package -->
<maintainer email="your_email@example.com">Your Name</maintainer>
<!-- License of the package -->
<license>MIT</license>
<!-- Build tool required to build this package -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies of the package during build and runtime -->
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>eigen</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>message_filters</build_depend>
<build_depend>image_transport</build_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>eigen</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>message_filters</exec_depend>
<exec_depend>image_transport</exec_depend>
<!-- Declare additional dependencies required for building this package -->
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>eigen</build_export_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>message_filters</build_export_depend>
<build_export_depend>image_transport</build_export_depend>
<!-- Export information, can be used by other packages -->
<export>
<!-- Export any specific information here -->
</export>
</package>
创建工程
发布节点
image_pose_publisher.cpp
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/Image.h>
#include <map>
#include <utility> // for std::pair
#include <mutex>
#include <thread>
#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>
#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry> // For Quaterniond
// 存储图像和Pose的队列
std::map<ros::Time, std::pair<sensor_msgs::ImageConstPtr, geometry_msgs::PoseStampedConstPtr>> message_queue;
std::mutex queue_mutex; // 互斥锁，用于保护message_queue
ros::Publisher pose_pub;
ros::Subscriber image_sub;
ros::Subscriber pose_sub;
void imageCallback(const sensor_msgs::Image::ConstPtr& img_msg)
{
std::lock_guard<std::mutex> lock(queue_mutex); // 上锁
// 查找对应的Pose
auto it = message_queue.find(img_msg->header.stamp);
double seconds = img_msg->header.stamp.toSec();
if (it != message_queue.end())
{
// 找到匹配的Pose
ROS_INFO("图像数据到达，存在匹配的pose，更新存入图像数据 %f", seconds);
// 这里可以处理图像和Pose
// std::pair<sensor_msgs::ImageConstPtr, geometry_msgs::PoseStampedConstPtr> paired_data = it->second;
// processImageAndPose(paired_data.first, paired_data.second);
message_queue[img_msg->header.stamp].first = img_msg;
// 移除匹配的消息对
//message_queue.erase(it);
}
else
{
ROS_INFO("图像数据到达，没有匹配的pose，更新存入图像数据 %f", seconds);
// 如果找不到对应的Pose，将图像存入队列
message_queue[img_msg->header.stamp].first = img_msg;
}
}
void poseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
//ROS_INFO("Received Pose data");
std::lock_guard<std::mutex> lock(queue_mutex); // 上锁
Eigen::Vector3d vio_t(pose_msg->pose.position.x, pose_msg->pose.position.y, pose_msg->pose.position.z);
Eigen::Quaterniond vio_q;
vio_q.w() = pose_msg->pose.orientation.w;
vio_q.x() = pose_msg->pose.orientation.x;
vio_q.y() = pose_msg->pose.orientation.y;
vio_q.z() = pose_msg->pose.orientation.z;
// 打印 Pose 信息
ROS_INFO("Received Pose - x: %f, y: %f, z: %f",
pose_msg->pose.position.x,
pose_msg->pose.position.y,
pose_msg->pose.position.z);
//td::unique_lock<std::mutex> lock(queue_mutex);
//data_queue.push(std::make_pair(image, pose_msg));
//lock.unlock();
// 查找对应的图像
auto it = message_queue.find(pose_msg->header.stamp);
double seconds = pose_msg->header.stamp.toSec();
if (it != message_queue.end()) //
{
// 找到匹配的图像
ROS_INFO("pose数据到达，存在匹配的图像，更新存入pose数据 %f", seconds);
// 这里可以处理图像和Pose
// std::pair<sensor_msgs::ImageConstPtr, geometry_msgs::PoseStampedConstPtr> paired_data = it->second;
// processImageAndPose(paired_data.first, paired_data.second);
message_queue[pose_msg->header.stamp].second = pose_msg;
// 移除匹配的消息对
//message_queue.erase(it);
}
else
{
ROS_INFO("pose数据到达，没有匹配的图像，暂时存入pose数据 %f", seconds);
// 如果找不到对应的图像，将Pose存入队列
message_queue[pose_msg->header.stamp].second = pose_msg;
}
}
void publishPose()
{
// // 旋转角度 (90 度转换为弧度)
// double theta = M_PI / 2; // 90 度是 π/2 弧度
// // 绕 z 轴旋转的旋转矩阵
// R << cos(theta), -sin(theta), 0,
// sin(theta), cos(theta), 0,
// 0, 0, 1;
Eigen::Matrix4d WGPS_T_WVIO;// vo坐标变换到gps坐标
WGPS_T_WVIO << 0, -1, 0, 1,
1, 0, 0, 2,
0, 0, 1, 3,
0, 0, 0, 1;
Eigen::Matrix3d R = WGPS_T_WVIO.block<3,3>(0,0); // 提取3x3的旋转矩阵
Eigen::Vector3d t = WGPS_T_WVIO.block<3,1>(0,3); // 提取平移向量
Eigen::Quaterniond quat(R); // 使用旋转矩阵构造四元数
// 输出结果
// std::cout << "Rotation Matrix R:\n" << R << std::endl;
// std::cout << "Translation Vector t:\n" << t << std::endl;
// std::cout << "Quaternion:\n"
// << " w: " << quat.w()
// << " x: " << quat.x()
// << " y: " << quat.y()
// << " z: " << quat.z()
// << std::endl; // 四元数的系数
geometry_msgs::PoseStamped pose_msg;
pose_msg.header.stamp = ros::Time::now(); // 设置时间戳
pose_msg.header.frame_id = "base_link"; // 设置坐标框架
pose_msg.pose.position.x = t[0]; // 示例位置
pose_msg.pose.position.y = t[1];
pose_msg.pose.position.z = t[2];
pose_msg.pose.orientation.x = quat.x(); // 示例姿态
pose_msg.pose.orientation.y = quat.y();
pose_msg.pose.orientation.z = quat.z();
pose_msg.pose.orientation.w = quat.w();
ROS_INFO("send Pose - x: %f, y: %f, z: %f",
pose_msg.pose.position.x,
pose_msg.pose.position.y,
pose_msg.pose.position.z);
// 发布PoseStamped消息
pose_pub.publish(pose_msg);
}
void displayThread()
{
cv::namedWindow("Image", cv::WINDOW_AUTOSIZE); // 创建一个窗口用于显示图像
while (ros::ok())
{
std::this_thread::sleep_for(std::chrono::milliseconds(10)); // 暂停以减少CPU使用率
std::lock_guard<std::mutex> lock(queue_mutex); // 上锁以保护对 message_queue 的访问
if (message_queue.empty()) {
ROS_INFO("Message queue is empty.");
cv::waitKey(1000);
}
else {
for (auto it = message_queue.begin(); it != message_queue.end();)
{
const auto& [timestamp, paired_data] = *it;
const sensor_msgs::ImageConstPtr& img_msg = paired_data.first;
const geometry_msgs::PoseStampedConstPtr& pose_msg = paired_data.second;
double seconds = timestamp.toSec();
if (img_msg && pose_msg)
//if(1)
{
ROS_INFO("Message queue img_msg和pose_msg数据同时满足，刷新图像 %f", seconds);
try
{
// 将ROS图像消息转换为OpenCV图像
cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::BGR8);
cv::imshow("Image", cv_ptr->image);
// 打印Pose信息
ROS_INFO("Pose - x: %f, y: %f, z: %f",
pose_msg->pose.position.x,
pose_msg->pose.position.y,
pose_msg->pose.position.z);
cv::waitKey(1); // 等待1毫秒以处理图像显示
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
cv::waitKey(1);
}
// 移除已处理的消息对
it = message_queue.erase(it);
}
else
{
//ROS_INFO("Message queue 数据不同时满足. 切换下一个");
++it;
}
}
}
}//while
cv::destroyAllWindows(); // 关闭所有OpenCV窗口
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "combined_node");
ros::NodeHandle nh;
// 创建发布者
pose_pub = nh.advertise<geometry_msgs::PoseStamped>("pose_topic", 10);
// 创建订阅者
image_sub = nh.subscribe("image_topic", 1, imageCallback);
pose_sub = nh.subscribe("pose_image_topic", 1, poseCallback);
// 定时器，用于定期发布PoseStamped消息
// ros::Timer timer = nh.createTimer(ros::Duration(1.0), [](const ros::TimerEvent&)
// {
// publishPose();
// });
//ros::spin();
// 启动显示线程
std::thread display_thread(displayThread);
// 定时器每秒调用一次
ros::Rate rate(1);
while (ros::ok())
{
publishPose();
// 调用ROS处理回调函数
ros::spinOnce();
rate.sleep();
}
// 确保线程在节点退出时正确终止
display_thread.join();
return 0;
}
接受节点
python这个节点随意路径安放，不需要任何配置，一个python直接跑，不像c++定义一堆东西和路径功能包，放在这里是为了好维护。
image_gps_subscriber.py
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped
from cv_bridge import CvBridge, CvBridgeError
from collections import deque
class PoseImagePublisher:
def __init__(self):
self.pose_sub = rospy.Subscriber('pose_topic', PoseStamped, self.pose_callback)
self.image_pub = rospy.Publisher('image_topic', Image, queue_size=10)
self.pose_pub = rospy.Publisher('pose_image_topic', PoseStamped, queue_size=10)
self.bridge = CvBridge()
self.pose_queue = deque() # Queue to store pose messages with timestamps
def pose_callback(self, msg):
# Store the pose message with timestamp in the queue
self.pose_queue.append((msg.header.stamp, msg))
print("收到 x", msg.pose.position.x, "y", msg.pose.position.y, "z", msg.pose.position.z)
def publish_image_with_pose(self):
rate = rospy.Rate(1) # 1 Hz
while not rospy.is_shutdown():
if self.pose_queue:
timestamp, pose_msg = self.pose_queue.popleft()
#random_x = np.random.uniform(-10, 10)
#x=random_x
x = pose_msg.pose.position.x
y = pose_msg.pose.position.y
z = pose_msg.pose.position.z
qx = pose_msg.pose.orientation.x
qy = pose_msg.pose.orientation.y
qz = pose_msg.pose.orientation.z
qw = pose_msg.pose.orientation.w
# Create an image
random_image = np.random.randint(0, 256, (480, 640, 3), dtype=np.uint8)
cv2.putText(random_image, f'Translation: ({x:.2f}, {y:.2f}, {z:.2f})', (10, 30),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
cv2.putText(random_image, f'Rotation: ({qx:.2f}, {qy:.2f}, {qz:.2f}, {qw:.2f})', (10, 60),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 1, cv2.LINE_AA)
try:
# Set the timestamp for the image and pose
image_msg = self.bridge.cv2_to_imgmsg(random_image, "bgr8")
image_msg.header.stamp = timestamp
pose_msg.header.stamp = timestamp
# Publish pose and image
self.pose_pub.publish(pose_msg)
self.image_pub.publish(image_msg)
print("图像数据发送", " 位姿xyz ", x, y, z)
except CvBridgeError as e:
rospy.logerr(f'CvBridge Error: {e}')
rate.sleep()
def main():
rospy.init_node('node2', anonymous=True)
pose_image_publisher = PoseImagePublisher()
pose_image_publisher.publish_image_with_pose()
if __name__ == '__main__':
main()
浙公网安备 33010602011771号