ROS在rviz中实时显示轨迹(nav_msgs/Path消息的使用)
消息结构说明
nav_msgs/Path.msg结构
#An array of poses that represents a Path for a robot to follow
Header header
geometry_msgs/PoseStamped[] poses
1
2
3
geometry_msgs/PoseStamped.msg结构
# A Pose with reference coordinate frame and timestamp
Header header
Pose pose
1
2
3
geometry_msgs/Pose.msg结构
# A representation of pose in free space, composed of position and orientation.
Point position
Quaternion orientation
1
2
3
geometry_msgs/Point.msg结构
# This contains the position of a point in free space
float64 x
float64 y
float64 z
1
2
3
4
geometry_msgs/Quaternion.msg结构
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
1
2
3
4
5
6
实现代码:
#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 <tf/transform_broadcaster.h>
#include <tf/tf.h>
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::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 th = 0.0;
double vx = 0.1;
double vy = -0.1;
double vth = 0.1;
ros::Rate loop_rate(1);
while (ros::ok())
{
current_time = ros::Time::now();
//compute odometry in a typical way given the velocities of the robot
double dt = (current_time - last_time).toSec();
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;
th += delta_th;
geometry_msgs::PoseStamped this_pose_stamped;
this_pose_stamped.pose.position.x = x;
this_pose_stamped.pose.position.y = 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);
ros::spinOnce(); // check for incoming messages
last_time = current_time;
loop_rate.sleep();
}
return 0;
}
---------------------
ROS在rviz中实时显示轨迹(nav_msgs/Path消息的使用)的更多相关文章
- Markdown中实时显示数学公式的方法
Markdown中实时显示数学公式的方法 Markdown非常好用,但是对于数学公式的实时显示有一些缺陷,如何解决这一问题呢? 一.在线LaTex编辑 点击在线LaTeX编辑方式 在对话框中输入数学公 ...
- 实时显示数据 SignalR 及时消息提醒( 立即向其推送内容)
实时显示数据 SignalR 及时消息提醒( 立即向其推送内容) http://www.cnblogs.com/Leo_wl/p/5634910.html <!--Reference the ...
- 【转】WriteMessage的信息在AutoCAD中命令行中实时显示
之前程序中有段发送信息到命令行上显示的代码,如下: ed.WriteMessage("开始标注横断面高程,请稍候!"); 但是发现命令行中并不马上显示,代码也明明运 ...
- 让Android Preference Summary中实时显示内容变更
Android中提供的Preference可以保存用户的喜好设置.在启明星安卓版员工通讯录里,有一个地方保存用户输入的URL就是用的Preference. 但是Preference默认显示的是Summ ...
- jquery+ajax 实现text框模糊搜索并可利用listbox实时显示模糊搜索列表结果
功能描述: text框中输入,text框下面的listbox中实时显示依据输入的内容进行模糊搜索的结果 js代码 $j(function() { $j("input[id='txtCos'] ...
- python websocket网页实时显示远程服务器日志信息
功能:用websocket技术,在运维工具的浏览器上实时显示远程服务器上的日志信息 一般我们在运维工具部署环境的时候,需要实时展现部署过程中的信息,或者在浏览器中实时显示程序日志给开发人员看.你还在用 ...
- JavaScript实时显示当前时间的例子
用javascript代码在当前页面中实时显示当前时间.代码如下: <html> <head> <title>JavaScript 实时显示当前时间-www.jbx ...
- 运维开发:python websocket网页实时显示远程服务器日志信息
功能:用websocket技术,在运维工具的浏览器上实时显示远程服务器上的日志信息 一般我们在运维工具部署环境的时候,需要实时展现部署过程中的信息,或者在浏览器中实时显示程序日志给开发人员看.你还在用 ...
- 将ROS中的/sensor_msgs/NavSatFix数据导入google earth显示轨迹
将ros中的gps_msg数据导入google earth显示轨迹 [TOC] 1. 获取GPS数据 将ros中发布的gps topic输出到文本中 rostopic echo -p /gpsData ...
随机推荐
- unbantu安装mysql
1,方法一 sudo apt-get install mysql-server apt isntall mysql-client apt install libmysqlclient-dev(暂不知用 ...
- 网站图片增强JS插件2.0(兼容IE&FF)
网站图片增强JS插件2.0简单介绍:插件可以增强网站互动能力与外链建设,用户在欣赏图片的同时,把看好的图片直接制作成自己喜欢的样式后通过QQ等传播,增强外链建设,通过用户互动创造外链.(支持:放大缩小 ...
- AutoIT: 如何通过坐标相对位置来对无法识别的Menu以及GridView进行定位点击操作
一般情况下,GridView中的数据来自数据库,我们通过Windows Info,是无法获取GridView中的信息的.而软件定制的Menu,很多时候无法通过系统提供的WinMenuSelectIte ...
- Java Socket编程如何建立两者关系
转自:http://developer.51cto.com/art/201003/190582.htm Java Socket编程需要大家详细的学习,有关的技术一直在不断的更新.那么要如何才能掌握好有 ...
- Hibernate4 拦截器(Interceptor) 实现实体类增删改的日志记录
转自:https://blog.csdn.net/he90227/article/details/44783099 开发应用程序的过程中,经常会对一些比较重要的数据修改都需要写日志.在实际工作的工程中 ...
- ASP.NET项目开发实战<<一键创建解决方案>>
视频演示地址:http://www.youku.com/playlist_show/id_23192838.html 第一步:创建项目需要的数据库 打开辅助开发工具,如下图 从左侧菜单找到 项目数据库 ...
- zoj3955:Saddle Point(想法题)
传送门 题意 给出n*m的矩阵,询问所有子矩阵中鞍点的个数 鞍点定义:在行唯一最小,在列唯一最大 分析 我们遍历每个点,计算该点对于答案的贡献即可. 每个点的贡献为\((2^{numa[i][j]}) ...
- Reduction operations
Reuction operations Reduction operations A reduction operations on a tensor is an operation that red ...
- 正睿多校联盟训练Week6
并没有参加 Problem A.阿瓦分蛋糕输入文件: cake.in输出文件: cake.out时间限制: 1 second空间限制: 512 megabytes阿瓦为了庆祝自己自己成长为了一只可爱的 ...
- ppt自动制作器
讲一篇论文导入进入,自动生成该论文的展示ppt,啧啧啧,想想岂不是很美妙