ROS Rviz 显示轨迹 Python

小咪咪 2022-10-22 01:40 189阅读 0赞

### ROS Rviz 显示轨迹 Python ###

*  1. 缘由
 *  2. Python实现
 *  3. 效果

--------------------

# 1. 缘由 #

3月一直在调试设备，还要持续一段时间，没空余时间  
工作上也遇到很多非技术问题  
同事的帮忙，最近状态才调整过来，看淡了

最近也有人来私信来问 ROS Rviz 怎么显示轨迹  
在网上的资料基本都是C++版本的  
“你行，我也行”，这里就补一下Python版本

--------------------

# 2. Python实现 #

#!/usr/bin/env python3
    
    
    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
    
    
    def DataUpdating(path_pub, path_record):
        """ 数据更新函数 """
        global x, y, th
    
        # 时间戳
        current_time = rospy.Time.now()
    
        # 发布tf
        br = tf.TransformBroadcaster()
        br.sendTransform((0.0, 0.0, 0.0), (0.0, 0.0, 0.0, 1.0),
                         rospy.Time.now(), "odom", "map")
    
        # 配置运动
        dt = 1 / 50
        vx = 0.25
        vy = 0.25
        vth = 0.2
        delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dt
        delta_y = (vx * math.sin(th) + vy * math.cos(th)) * dt
        delta_th = vth * dt
        x += delta_x
        y += delta_y
        th += delta_th
    
        # 四元素转换
        quat = tf.transformations.quaternion_from_euler(0, 0, th)
    
        # 配置姿态
        pose = PoseStamped()
        pose.header.stamp = current_time
        pose.header.frame_id = 'odom'
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.orientation.x = quat[0]
        pose.pose.orientation.y = quat[1]
        pose.pose.orientation.z = quat[2]
        pose.pose.orientation.w = quat[3]
    
        # 配置路径
        path_record.header.stamp = current_time
        path_record.header.frame_id = 'odom'
        path_record.poses.append(pose)
    
        # 路径数量限制
        if len(path_record.poses) > 1000:
            path_record.poses.pop(0)
    
        # 发布路径
        path_pub.publish(path_record)
    
    
    def node():
        """ 节点启动函数 """
        try:
    
            # 初始化节点path
            rospy.init_node('PathRecord')
    
            # 定义发布器 path_pub 发布 trajectory
            path_pub = rospy.Publisher('trajectory', Path, queue_size=50)
    
            # 初始化循环频率
            rate = rospy.Rate(50)
    
            # 定义路径记录
            path_record = Path()
    
            # 在程序没退出的情况下
            while not rospy.is_shutdown():
                # 数据更新函数
                DataUpdating(path_pub, path_record)
    
                # 休眠
                rate.sleep()
    
        except rospy.ROSInterruptException:
            pass
    
    
    if __name__ == '__main__':
        node()

--------------------

# 3. 效果 #

怎么启动上述文件就不再描述了  
直接上图看效果  
![Rviz 显示轨迹][Rviz]

--------------------

谢谢

[Rviz]: /images/20221022/8615c656692b411a841df033c81cab2d.png