1. 缘由

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

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

2. Python实现

#!/usr/bin/env python3import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped, Quaternion
import tf
import math# 起始运动状态
x, y, th = 0, 0, 0def DataUpdating(path_pub, path_record):"""数据更新函数"""global x, y, th# 时间戳current_time = rospy.Time.now()# 发布tfbr = 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 / 50vx = 0.25vy = 0.25vth = 0.2delta_x = (vx * math.cos(th) - vy * math.sin(th)) * dtdelta_y = (vx * math.sin(th) + vy * math.cos(th)) * dtdelta_th = vth * dtx += delta_xy += delta_yth += delta_th# 四元素转换quat = tf.transformations.quaternion_from_euler(0, 0, th)# 配置姿态pose = PoseStamped()pose.header.stamp = current_timepose.header.frame_id = 'odom'pose.pose.position.x = xpose.pose.position.y = ypose.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_timepath_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:# 初始化节点pathrospy.init_node('PathRecord')# 定义发布器 path_pub 发布 trajectorypath_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:passif __name__ == '__main__':node()

3. 效果

怎么启动上述文件就不再描述了
直接上图看效果

谢谢

ROS Rviz 显示轨迹 Python相关推荐

ROS Rviz 显示地图 Python
ROS Rviz 显示地图 Python 1. 缘由 2. Python实现 3. 效果 1. 缘由地图在自主移动机器人这个行业里是非常重要的第一步需要可视化地图来感官了解一下在网上的资料基本都 ...
ROS Rviz 显示超声波测量范围 Python
ROS Rviz 显示超声波测量范围 Python 1. 缘由 2. Python实现 3. 效果最近几个月都在忙调试和工程的事情太忙了,两个月都没发布文章了过阵子应该就可以重新开始软件开发工作 ...
ROS入门:GPS坐标转换Rviz显示轨迹
GPS信息是无法直接绘制轨迹的,因为其x,y为经纬度,z为高度,单位不一样,本程序实现了以下功能: 1.将GPS轨迹,从经纬度WGS-84坐标转换到真实世界xyz坐标系下(思路:计算出每个 ...
小场景下基于ROS的GPS经纬高度值转换为平面XYZ坐标值，并用RVIZ显示轨迹
一.实现原理在小范围场景下,可以假设GPS经纬度值都在一个平面上,地理正东方向为经度正方向,正北方向为纬度正方向,正上方向为高度正方向,至此经纬高度坐标系已经建立.而我们要做的是将其转换到一个以米为 ...
ros rviz显示rosbag中的图像和imu数据
一.rosbag相关的指令 1. rostopic list //列举出系统中正在发布的ros 话题 2. rosbag record -a //录制系统中所有正在发布的ros 话题 3. rosba ...
ROS+Turtlebot3+RVIZ 显示机器人历史轨迹方法
(六)ROS发布里程计(Odometry)消息并在rviz中显示_火星机器人life的博客-CSDN博客_ros 发布odom 一.如果只是RVIZ查看移动机器人的历史轨迹方法 1. 打开gazebo ...
Rviz显示理想的运动的轨迹，并对比实时的运动轨迹
20210505更新前天的bug,修改了marker点的尺寸和颜色后在rviz中不显示的问题解决了,代码什么的完全没有改,只是用了sudo apt-get upgrade,把ros的一些包和库都升级 ...
ROS中rviz显示运动轨迹的常见方法
文章目录 1.利用nav_msgs/Path消息实现轨迹显示 2.利用visualization_msgs/Marker消息类型显示轨迹 1.利用nav_msgs/Path消息实现轨迹显示主函数sh ...
ros中启动rviz显示段错误，核心以转储问题
ros中启动rviz显示段错误,核心以转储问题运行命令 $ rosrun rviz rviz -d rospack find turtle_tf/rviz/turtle_rviz.rviz 显示解 ...

ROS Rviz 显示轨迹 Python

ROS Rviz 显示轨迹 Python

1. 缘由

2. Python实现

3. 效果

ROS Rviz 显示轨迹 Python相关推荐

最新文章

热门文章