ROS接收rviz中的2D Nav Goal消息

最新推荐文章于 2024-10-21 20:38:59 发布

一只小白白

最新推荐文章于 2024-10-21 20:38:59 发布

阅读量1.6k

点赞数 1

分类专栏： ROS初探文章标签： c++ ubuntu

本文链接：https://blog.csdn.net/qq_41701758/article/details/132072074

版权

ROS初探专栏收录该内容

5 篇文章

订阅专栏

文章目录

1 2D Nav Goal消息数据类型查看
2 订阅目标点消息并打印（C++）
3 测试

1 2D Nav Goal消息数据类型查看

终端输入：roscore
另开终端：rviz
另开终端：输入：rostopic list
可以看到显示的话题有：
在这里插入图片描述
继续输入：rostopic type /move_base_simple/goal
可以看到消息类型如下：

继续输入：rosmsg show geometry_msgs/PoseStamped
可以看到消息数据类型如下：

2 订阅目标点消息并打印（C++）

2.1 创建功能包

mkdir -p navgoal_ws/src
cd navgoal_ws
catkin_make
cd src
catkin_create_pkg navgoal_rviz roscpp rospy std_msgs nav_msgs geometry_msgs

2.2 编写代码

navgoal/src/下新建nav_goal.cpp文件，代码如下：

#include "ros/ros.h"
#include "std_msgs/String.h"
#include <iostream>
#include <geometry_msgs/PoseStamped.h>
#include <tf/tf.h>
 
void Callback(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
    double x=msg->pose.position.x;
    double y=msg->pose.position.y;
    //四元数转欧拉角
    tf::Quaternion quat;
    tf::quaternionMsgToTF(msg->pose.orientation, quat);
    double roll, pitch, yaw;//定义存储r\p\y的容器
    tf::Matrix3x3(quat).getRPY(roll, pitch, yaw);//进行转换
    //打印输出
    std::cout<<"pos-x: "<<x<<"pos-y: "<<y<<"pos-yaw: "<<yaw<<std::endl;
}
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "navgoal_rviz");
    ros::NodeHandle nh;
 
    ros::Subscriber sub = nh.subscribe("/move_base_simple/goal", 10, Callback);//队列长度:1000或1或其他
 
    while(ros::ok())
    {
        ros::spinOnce();
    }
 
  return 0;
}

2.3 CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(navgoal_rviz)

## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)

## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  nav_msgs
  roscpp
  rospy
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES using_markers
#  CATKIN_DEPENDS roscpp visualization_msgs
#  DEPENDS system_lib
)

## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)

add_executable(${PROJECT_NAME}_node src/nav_goal.cpp) #${PROJECT_NAME}_node
target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES}) # ${PROJECT_NAME}_node

2.4 编译

cd ~/namgoal_ws/
catkin_make
source devel/setup.bash

3 测试

打开终端输入：roscore
另开终端输入：rosrun navgoal_rviz navgoal_rviz_node
另开终端输入：rviz
在rviz中点击2D nav goal后再点击rviz可视化位置发布信息即可，在终端上会显示打印的位置信息如下：
在这里插入图片描述