ROS在rviz中实时显示轨迹和点

记录一个简单的例子方便复制：
通过使用

nav_msgs::Path
geometry_msgs::PoseStamped
geometry_msgs::PointStamped

来实现rviz中可视化轨迹和轨迹点。
首先创建工程：

mkdir -p showpath/src
cd src
catkin_create_pkg showpath roscpp rospy sensor_msgs std_msgs nav_msgs tf
cd ..
catkin_make

以及修改cmakelist:

add_executable(showpath src/showpath.cpp)
target_link_libraries(showpath ${catkin_LIBRARIES})

编译和运行：

catkin build
source ./devel/setup.bash//别忘了sourceroscore
rosrun showpath showpath
rostopic echo /trajectory
rostopic echo /point
rosrun rviz rviz

代码部分：

#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/Path.h>
#include <std_msgs/String.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <cstdlib>main(int argc, char **argv)
{ros::init(argc, argv, "showpath");ros::NodeHandle ph;ros::Publisher path_pub = ph.advertise<nav_msgs::Path>("trajectory", 1, true);ros::Publisher point_pub = ph.advertise<geometry_msgs::PointStamped>("point", 1, true);ros::Time current_time, last_time;current_time = ros::Time::now();last_time = ros::Time::now();nav_msgs::Path path;//nav_msgs::Path path;path.header.stamp = current_time;path.header.frame_id = "odom";double x = 0.0;double y = 0.0;double z = 0.0;double th = 0.0;double vx = 0.1 + 0.2 * rand() / double(RAND_MAX);double vy = -0.1 + 0.2 * rand() / double(RAND_MAX);double vth = 0.1;ros::Rate loop_rate(10);while (ros::ok()){current_time = ros::Time::now();//compute odometry in a typical way given the velocities of the robotdouble dt = 1;double delta_x = (vx * cos(th) - vy * sin(th)) * dt;double delta_y = (vx * sin(th) + vy * cos(th)) * dt;double delta_th = vth * dt;x += delta_x;y += delta_y;z += 0.005;th += delta_th;geometry_msgs::PoseStamped this_pose_stamped;geometry_msgs::PointStamped this_point_stamped;this_pose_stamped.pose.position.x = x;this_pose_stamped.pose.position.y = y;this_pose_stamped.pose.position.z = z;this_point_stamped.header.stamp = current_time;this_point_stamped.header.frame_id = "odom";this_point_stamped.point.x = x;this_point_stamped.point.y = y;this_point_stamped.point.z = z;ROS_INFO("current_x: %f", x);ROS_INFO("current_y: %f", y);geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(th);this_pose_stamped.pose.orientation.x = goal_quat.x;this_pose_stamped.pose.orientation.y = goal_quat.y;this_pose_stamped.pose.orientation.z = goal_quat.z;this_pose_stamped.pose.orientation.w = goal_quat.w;this_pose_stamped.header.stamp = current_time;this_pose_stamped.header.frame_id = "odom";path.poses.push_back(this_pose_stamped);path_pub.publish(path);point_pub.publish(this_point_stamped);ros::spinOnce(); // check for incoming messageslast_time = current_time;loop_rate.sleep();}return 0;
}

rviz中globel option的Fixed Fram输入odom
左边点击add
选中path
在path的topic选项中选
/trajectory和/point

add对应topics

修改对齐frame_id的odom

选择trajectory和 point

效果如下：

ROS在rviz中实时显示轨迹和点相关推荐

基于pyqt5 构建弹窗进度条，在大型计算中实时显示进度
在大型计算时(例如神经网络训练),经常会遇到计算时间过长,无法知道当前的计算进度,无法判断程序是否进入死循环等问题.采用进度条可以在一定程度上了解当前进度,判断后续所需的计算时间,缓解等待过程中的焦虑 ...
Gmapping：rviz中map显示no map received
Gmapping:rviz中map显示no map received 问题描述本次使用Gmapping算法在运行mapping.launch文件后,另启一个终端打开rviz,在Add map后发现m ...
ROS学习之tf在rviz中的显示
昨天粗略看了一下ros中rviz的用法.事实,他就是一个可视化的工具.wiki的用户手册:http://wiki.ros.org/rviz/UserGuide 今天在学习tf的教程时,首先是演示了一个 ...
Franka Emika Panda机械臂规划路径时，rviz中手爪显示碰撞
[Franka Emika Panda连接真实机械臂(二)] 本文是对上文的一些补充. 前文中的franka_ros功能包和panda_moveit_config功能包是分别在不同的工作空间编译的,所 ...
【Unity】在Unity中实时显示北京时间
感觉在网上搜到的大部分Unity或者C#获取北京时间的方法都已经不提供服务了,搜到一个可用的稍微拓展下做成了实时显示北京时间的脚本. 但因为只在程序启动的时候有获取北京时间,接下来显示的时间都是每秒钟 ...
Cesium中实时显示经纬度及视角高
如何在cesium中实时根据鼠标的位置显示经纬度以及视角高具体效果如下: 具体代码如下: //html代码 <div id="latlng_show" style=&quo ...
ros moveit rviz 中轨迹球消失了怎么办？
轨迹球消失不见了,无法拖拽机械臂末端,有可能是这个interactive marker size被设置为0了,把它设置成0.2就可以轨迹球出现
在TextView中实时显示数据
//下面实时更新显示在textView中的数据handler = new Handler();runnable = new Runnable(){@Overridepublic void run(){ ...
ROS-PCL读取pcd点云数据并在rviz中进行显示
环境: Ubuntu18.04 ROS melodic C++ 创建工作空间和功能包 cd Downloads/ROS mkdir -p pcdreadshow_ws/src cd src catki ...

ROS在rviz中实时显示轨迹和点

ROS在rviz中实时显示轨迹和点相关推荐

最新文章

热门文章