ros(2) 模拟slam定位和高斯渲染通信

MKT-porter / 2024-08-24 / 原文

节点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()