ROS Rviz 显示轨迹 Python

其他 专栏收录该内容
4 篇文章 0 订阅

ROS Rviz 显示轨迹 Python


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 显示轨迹


谢谢

  • 3
    点赞
  • 14
    评论
  • 5
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

打赏
文章很值,打赏犒劳作者一下
相关推荐
©️2020 CSDN 皮肤主题: 我行我“速” 设计师:Amelia_0503 返回首页

打赏

氢键H-H

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值