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

随笔- 1525 文章- 2 评论- 67 阅读- 128万

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

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

189

190

191

192

193

194

195

196

197

198

199

200

201

202

203

204

205

206

207

208

209

210

211

212

213

214

215

216

217

218

219

220

221

222

223

224

225

226

227

228

229

230

231

232

233

234

235

236

237

238

239

240

241

242

243

244

245

246

247

248

249

250

251

252

253

254

255

256

#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()