1. 首先查看设置初始坐标的话题为 /intialpose ，查看消息类型和格式从而决定怎么给它发数据

（1）首先打开一个可以自动导航的项目文件，打开rviz，点击2D Pose Estimate 进行初始位姿矫正，查看/initialpose消息格式：

sun@sun-pc:~$ rostopic info /initialpose
Type: geometry_msgs/PoseWithCovarianceStampedPublishers: * /rviz (http://sun-pc:40347/)Subscribers: * /amcl (http://sun-pc:35701/)

（2）然后查看消息数据格式

sun@sun-pc:~$ rosmsg show geometry_msgs/PoseWithCovarianceStamped
std_msgs/Header headeruint32 seqtime stampstring frame_id
geometry_msgs/PoseWithCovariance posegeometry_msgs/Pose posegeometry_msgs/Point positionfloat64 xfloat64 yfloat64 zgeometry_msgs/Quaternion orientationfloat64 xfloat64 yfloat64 zfloat64 wfloat64[36] covariance

2.监听rviz发出的数据格式

sun@sun-pc:~$ rostopic echo /initialpose
WARNING: no messages received and simulated time is active.
Is /clock being published?
header: seq: 2stamp: secs: 825nsecs: 700000000frame_id: "map"
pose: pose: position: x: 39.8066101074y: 41.3922195435z: 0.0orientation: x: 0.0y: 0.0z: -0.0116650747515w: 0.999931960701covariance: [ 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
---

3.仿照rviz消息格式

position：为坐标信息，对2d来说只有x和y值。

orientation为四元数格式，参考相关链接：四元数与欧拉角之间的转换

对2d平面的移动机器人感官上易于理解的就是朝向信息，即欧拉角中绕z轴旋转的偏航角。

俯仰角和滚转角为0，故x和y均为0，即只有w和z值。

若偏行角为alpha，则w = cos（alpha/2），z = sin（alpha/2）。

4.编写发布节点程序

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "math.h"
#define PI 3.1415926int main(int argc, char **argv)
{ros::init(argc, argv, "pose_estimate_2d");ros::NodeHandle nh;ros::Publisher initial_pose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose", 10);ros::Rate loop_rate(1);//define 2d estimate posedouble alpha = PI/2;//radian valuedouble x_pos = 43.0231246948;double y_pos = 41.5323944092;while (ros::ok()){geometry_msgs::PoseWithCovarianceStamped pose_msg;pose_msg.header.stamp = ros::Time::now();pose_msg.header.frame_id = "map";pose_msg.pose.pose.position.x = x_pos;pose_msg.pose.pose.position.y = y_pos;pose_msg.pose.covariance[0] = 0.25;pose_msg.pose.covariance[6 * 1 + 1] = 0.25;pose_msg.pose.covariance[6 * 5 + 5] = 0.06853891945200942;pose_msg.pose.pose.orientation.z = sin(alpha/2);pose_msg.pose.pose.orientation.w = cos(alpha/2);initial_pose_pub.publish(pose_msg);ROS_INFO("Setting to :(%f,%f)",x_pos,y_pos);ros::spinOnce();loop_rate.sleep();}return 0;
}

欢迎留言交流～

C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标相关推荐

rviz导航——2D Pose estimate
关于rviz中小车初始点的设置问题一般有两种方法: 1打开rviz 在其上方工具栏中有2D Pose estimate,用来设置大概的初始点 2一般在amcl.launch文件中也会定义初始点,大多 ...
导航中通过“initialpose“设置机器人位姿
rviz中 2D Pose Estimate如何设定机器人的位姿 amcl功能包中 amcl_node.cpp initial_pose_sub_ = nh_.subscribe("init ...
基于深度学习的3D pose estimation总结（包括几篇2D pose estimation）
声明:此文章是作者自己学习过程中的简单总结,也是作者第一次上传,仅供各位同行参考,也请对文中错误和不足多多指教,敬请指正,谢谢! 一.任务描述给定一幅图或者是一段视频,人体姿态估计就是恢复出其中的人 ...
[OpenPose翻译] Realtime Multi-Person 2D Pose Estimation using Part Affinity Fields ∗原文翻译（注释版）
转载自here, 写的非常好. 论文原文地址:https://arxiv.org/abs/1611.08050(在网页右上角附近,有"PDF"字样,点击即可下载) 本论文有提供代码 ...
人体姿态估计(人体关键点检测)2D Pose训练代码和Android源码
人体姿态估计(人体关键点检测)2D Pose训练代码和Android源码目录人体姿态估计(人体关键点检测)2D Pose训练代码和Android源码 1.人体姿态估计2D Pose方法 2.人体姿 ...
行人姿态估计--Realtime Multi-Person 2D Pose Estimation using Part Affinity Fields
Realtime Multi-Person 2D Pose Estimation using Part Affinity Fields CVPR 2017 Code: https://github.c ...
商城网站前期功能设置这几项绝不能少！
目前绝大多数的商城网站在基本建设情况下都是以用户需求为主,面对不一样的商品和不一样的市场行情,各行各业都各显神通促进商城网站的有效开展和制作,那么在商城网站设计构思之初,有哪些功能是一定不能少的呢? ...
Word的常用功能设置（
Word的常用功能设置(20181229) 一.Word中中文输入为宋体,英文输入为Times New Roman实现自动切换二.打造常用的word模板三.word中的行间距问题一.Word中中 ...
Geany 代码自动补齐功能设置
很多人用 geany 感觉不方便,有一部分就是相比于 pycham 的自动补齐功能不好使. 现在给大家看下自动补齐功能设置方法,设置完后用起来非常方便哦. Geany 代码自动补全设置: 默认出现自动 ...

C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标

1. 首先查看设置初始坐标的话题为 /intialpose ，查看消息类型和格式从而决定怎么给它发数据

2.监听rviz发出的数据格式

3.仿照rviz消息格式

4.编写发布节点程序

C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标相关推荐

最新文章

热门文章

C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标

1. 首先查看设置初始坐标的话题 为 /intialpose ，查看消息类型和格式从而决定怎么给它发数据

2.监听rviz发出的数据格式

3.仿照rviz消息格式

4.编写发布节点程序

C++实现rviz 2D Pose Estimate 功能设置机器人初始坐标相关推荐

最新文章

热门文章

1. 首先查看设置初始坐标的话题为 /intialpose ，查看消息类型和格式从而决定怎么给它发数据