ros(2) 模擬slam定位和高斯渲染通訊

MKT-porter發表於2024-08-24
ROSSLAM

節點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()

　　

相關文章