ros vrrp 改mac（ros(23)：接收rviz中的2D Nav Goal、2D Pose Estimate消息）

1 rviz 教程

1.1 2D Nav Goal

2D Nav Goal (Keyboard shortcut: g)

This tool lets you set a goal sent on the "goal" ROS topic. Click on a location on the ground plane and drag to select the orientation:

二维导航目标（快捷键：g）

此工具允许您设置在“goal 
  
  
  
  
 ”ROS主题上发送的目标 
  
  
  
  
 。单击地平面上的某个位置并拖动以选择方向：

即设置二维导航目标 
  
  
  
  
 ，并使用“goal
  
  
  
  
  ”这个话题进行通讯（结合rviz的其他教程
  
  
  
  
  ，话题名也可能是“/move_base_simple/goal
 


 


 


 


 

”）

其消息类型为：geometry_msgs/PoseStamped

meng@meng:~/ideas/ros_ws$ rosmsg info geometry_msgs/PoseStamped std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w

1.2 2D Pose Estimate

2D Pose Estimate (Keyboard shortcut: p)

This tool lets you set an initial pose to seed the localization system (sent on the "initialpose" ROS topic). Click on a location on the ground plane and drag to select the orientation:

二维姿势估计（键盘快捷键：p）

此工具允许您设置初始姿势以播种定位系统（发送至“initialpose 
 
 
 
 
”ROS主题）
  
  
  
  
  。单击地平面上的某个位置并拖动以选择方向：

即设置二维初始位姿并使用“initialpose
   
   
   
   
   ”进行通讯

其消息类型为：geometry_msgs/PoseWithCovarianceStamped

meng@meng:~/ideas/ros_ws$ rosmsg info geometry_msgs/PoseWithCovarianceStamped std_msgs/Header header uint32 seq time stamp string frame_id geometry_msgs/PoseWithCovariance pose geometry_msgs/Pose pose geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64[36] covariance

1.3 打开rviz查看

Panels--Tool Properties（勾选）

2 订阅话题

订阅起点位姿和终点话题并打印输出的c++文件：receive_2d_nav_goal.cpp

#include "ros/ros.h" #include "std_msgs/String.h" #include <geometry_msgs/PoseWithCovarianceStamped.h> #include <iostream> #include <geometry_msgs/PoseStamped.h> void chatterCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) { double x=msg->pose.pose.position.x; double y=msg->pose.pose.position.y; std::cout<<x<<y<<std::endl; } void chatterCallback1(const geometry_msgs::PoseStamped::ConstPtr& msg) { std::cout<<"1111"<<std::endl; double x=msg->pose.position.x; double y=msg->pose.position.y; std::cout<<x<<y<<std::endl; } int main(int argc, char **argv) { ros::init(argc, argv, "reveive_rviz"); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe("/initialpose", 1, chatterCallback);//队列长度:1000或1或其他 ros::Subscriber sub1 = nh.subscribe("/move_base_simple/goal", 1, chatterCallback1);//队列长度:1000或1或其他 while(ros::ok()) { ros::spinOnce(); } return 0; }

启动rviz和节点程序
 


 


 


 


 

，用 2D Nav Goal          、2D Pose Estimate 在rviz中做标记 
 
 
 
 
，即可打印输出:

订阅起点位姿和终点
   
   
   
   
   ，并保持发布：

#include "ros/ros.h" #include "std_msgs/String.h" #include <geometry_msgs/PoseWithCovarianceStamped.h> #include <iostream> #include <geometry_msgs/PoseStamped.h> ros::Publisher initialpose_pub,goal_pub; geometry_msgs::PoseWithCovarianceStamped initialpose_tmp;//设置为全局变量
 

 

 

 

 
，可以一直被发布出来 geometry_msgs::PoseStamped goal_tmp; void initialpose_handler(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) { double x=msg->pose.pose.position.x; double y=msg->pose.pose.position.y; std::cout<<"起点坐标：("<<x<<", "<<y<<")"<<std::endl; initialpose_tmp=*msg; // initialpose_tmp.header=msg->header; // initialpose_tmp.header=msg->header; initialpose_pub.publish(initialpose_tmp); } void goal_handler(const geometry_msgs::PoseStamped::ConstPtr& msg) { double x=msg->pose.position.x; double y=msg->pose.position.y; std::cout<<"终点坐标：("<<x<<", "<<y<<")"<<std::endl; goal_tmp=*msg; goal_pub.publish(*msg); } int main(int argc, char **argv) { ros::init(argc, argv, "reveive_rviz"); ros::NodeHandle nh; ros::Subscriber sub = nh.subscribe("/initialpose", 1, initialpose_handler);//1000改为1 ros::Subscriber sub1 = nh.subscribe("/move_base_simple/goal", 1, goal_handler);//1000改为1 initialpose_pub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("initialpose_my", 1); goal_pub = nh.advertise<geometry_msgs::PoseStamped>("goal_my", 1); while(ros::ok()) { ros::spinOnce(); } return 0; }

参考链接：

2D Nav Goal和2D Pose Estimate功能介绍：rviz/UserGuide - ROS Wiki

2D Nav Goal和2D Pose Estimate的消息类型：navigation/Tutorials/Using rviz with the Navigation Stack - ROS Wiki