ROS修改rosbag中话题的frame_id，以及发布坐标在Rviz中与点云融合显示

ROS修改rosbag中话题的frame_id，以及发布坐标在Rviz中与点云融合显示

1. 读取rosbag，修改话题frame_id与话题名并循环发布

import rosbag
import rospy
import numpy as np
from sensor_msgs.msg import PointCloud2

bag_file = 'cumt_1204_2022-12-04-20-01-08.bag'
bag = rosbag.Bag(bag_file, "r")
rospy.init_node('change_frameid')
rate = rospy.Rate(10)
while 1:
    bag_data = bag.read_messages('/laser_cloud_surround')
    for topic, msg, t in bag_data:
        msg.header.frame_id = 'livox'
        point_pub = rospy.Publisher('laser_surround', PointCloud2, queue_size=50)
        point_pub.publish(msg)
        rate.sleep()

2. 读取xlsx文件中的坐标并发布

#!/usr/bin/env python3

import xlrd
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Quaternion
import tf
import math

# 起始运动状态
x, y, th = 0, 0, 0
data = xlrd.open_workbook('data.xlsx')
sheet1 = data.sheets()[0]
# 数据总行数
nrows = sheet1.nrows
# 数据总列数
ncols = sheet1.ncols

def DataUpdating(path_pub, path_record):
    """
    数据更新函数
    """
    global x, y, th

    # 时间戳
    current_time = rospy.Time.now()

           
    # 配置运动
    

    #print(nrows, ncols)
    # print(sheet1.col_values(0)[0])

    
    
    # 四元素转换
    #quat = tf.transformations.quaternion_from_euler(0, 0, th)
    
    # 发布tf
    br = tf.TransformBroadcaster()
    #br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
                     #rospy.Time.now(), "/camera_init", "odom")       
    br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
                     rospy.Time.now(), "livox", "odom")
    # 配置姿态
    pose = PoseStamped()
    pose.header.stamp = current_time
    pose.header.frame_id = 'livox'
    pose.pose.position.x = x
    pose.pose.position.y = y
    pose.pose.orientation.x = 0
    pose.pose.orientation.y = 0
    pose.pose.orientation.z = 0
    pose.pose.orientation.w = 0


    # 配置路径
    path_record.header.stamp = current_time
    path_record.header.frame_id = 'livox'
    path_record.poses.append(pose)

    # 路径数量限制
    if len(path_record.poses) > 200:
        path_record.poses.pop(0)

    # 发布路径
    path_pub.publish(pose)


def node():
    """
    节点启动函数
    """
    try:
        # 初始化节点path
        rospy.init_node('PathRecord')
        # 定义发布器 path_pub 发布 trajectory
        path_pub = rospy.Publisher('trajectory', Path, queue_size=50)
        #point_pub = rospy.Publisher('point', PoseStamped, queue_size=50)
        # 初始化循环频率
        rate = rospy.Rate(10)
        # 定义路径记录
        path_record = Path()
        for i in range(1, nrows):
            # 时间戳
            current_time = rospy.Time.now()
            x=sheet1.cell(i,0).value
            y=sheet1.cell(i,1).value
            x=float(x)/1000
            y=float(y)/1000
            # 发布tf
            br = tf.TransformBroadcaster()
            #br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
                     #rospy.Time.now(), "/camera_init", "odom")       
            br.sendTransform((0, -1.6, 0.6), (0.001, 0.025, 0.032,  0.999),
                     rospy.Time.now(), "base", "livox")
            br.sendTransform((x, y, 0.0), (0.0, 0.0, 0.0,  1.0),
                     rospy.Time.now(), "odom", "base")
            # 配置姿态
            pose = PoseStamped()
            pose.header.stamp = current_time
            pose.header.frame_id = 'base'
            pose.pose.position.x = x
            pose.pose.position.y = y
            pose.pose.position.z = 0.0
            pose.pose.orientation.x = 0
            pose.pose.orientation.y = 0
            pose.pose.orientation.z = 0
            pose.pose.orientation.w = 1
            # 配置路径
            path_record.header.stamp = current_time
            path_record.header.frame_id = 'base'
            path_record.poses.append(pose)
            # 路径数量限制
            if len(path_record.poses) > 100:
                path_record.poses.pop(0)
            # 发布路径
            path_pub.publish(path_record)
        # 在程序没退出的情况下
        #while not rospy.is_shutdown():
            # 数据更新函数
            #DataUpdating(path_pub, path_record)
            # 休眠
            rate.sleep()
    except rospy.ROSInterruptException:
        pass
if __name__ == '__main__':
    node()

效果视频