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;
}
---------------------
相关文章
- rsync组合inotify-tools完成实时同步[转]
- 不需要服务器,教你仅用30行代码搞定实时健康码识别
- 简易的 Linux 流量实时监控工具 watch+ifstat
- 架构师必知的开源实时流处理系统
- 七牛基于“LiveNet”打造更实时、更智能、更真实的直播云
- 基于python的websocket开发,tomcat日志web页面实时打印监控案例
- 实时消息传输协议(RTMP)详解
- 程序员远程斗代码!网页版Skype推出内置实时代码编辑器
- 《构建实时机器学习系统》一第1章 实时机器学习综述 1.1 什么是机器学习
- 局域网实时聊天系统(JAVA)--利用Sock
- Flink大数据实时计算系列-Flink写出多个parquet小文件处理方法、Presto的介绍与使用场景
- Flink大数据实时计算系列-Flink的processFunction TimerService的基础用法
- Flink大数据实时计算系列-第一个程序JAVA版本-分组统计 funcation写法
- TRichView添加Android演示和实时拼写建议
- 集成友盟统计,添加测试设备,实时查看上报自定义事件
- 分布式实时消息队列Kafka(五)
- 分布式实时消息队列Kafka(四)
- 分布式实时消息队列Kafka(三)
- C#--耗时操作实现UI界面实时更新不阻塞(耗时操作解决窗体卡顿)
- linux 监控网卡实时流量iftop
- [Unity3d]使用摄像机制作实时显示小地图