ROS：订阅话题并发布（订阅amcl_pose数据并发布）

一、创建节点

 cd ~/catkin_ws/src
 catkin_create_pkg amcl_pose_sub_pub message_generation std_msgs roscpp
 roscd amcl_pose_sub_pub/src 	// → 移至src目录,该目录是功能包的源代码目录
 gedit amcl_pose_sub_pub.cpp 		// → 新建源文件并修改内容

amcl_pose_sub_pub.cpp

#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <iostream>
class amcl_pose_sub_pub {
private:
    ros::NodeHandle nh_;  // 定义ROS句柄
    ros::Publisher msgPointPub; 
    ros::Subscriber sub;

public:
    amcl_pose_sub_pub()
    {
    msgPointPub = nh_.advertise<geometry_msgs::PointStamped>("amcl_pose_", 1000);
    sub = nh_.subscribe("/amcl_pose", 1, &amcl_pose_sub_pub::amcl_pose_CallBack,this);
    }

    ~amcl_pose_sub_pub(){}

    void amcl_pose_CallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
    {
        geometry_msgs::PointStamped msgPointStamped;
        msgPointStamped.point=msg->pose.pose.position;
        msgPointStamped.header.stamp = msg->header.stamp;
        msgPointStamped.header.frame_id = "map";
        msgPointPub.publish(msgPointStamped);
        std::cout<<"amcl"<<std::endl;
    }
};
int main(int argc, char** argv)
{
    ros::init(argc, argv,"amcl_pose_sub_pub");
    amcl_pose_sub_pub obj;
    ros::spin();
    return 0;
}

在CMakeLists.txt文件中添加以下选项来生成可执行文件。add_executable(amcl_pose_sub_pub src/amcl_pose_sub_pub.cpp)也就是说,通过构建amcl_pose_sub_pub.cpp文件来创建amcl_pose_sub_pub可执行文件。

修改CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(amcl_pose_sub_pub)
find_package(catkin REQUIRED COMPONENTS
  message_generation
  roscpp
  std_msgs
)
catkin_package()
include_directories(include ${catkin_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})

include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)
add_executable(amcl_pose_sub_pub src/amcl_pose.cpp)
target_link_libraries(amcl_pose_sub_pub ${catkin_LIBRARIES})

二、rviz显示结果

运行导航：

roslaunch turtlebot3_gazebo turtlebot3_world.launch 
roslaunch turtlebot3_navigation turtlebot3_navigation.launch

运行节点：

rosrun amcl_pose_sub_pub amcl_pose_sub_pub 

记得在rviz中添加pointstanted订阅发布的amcl结果，其实已有对应于amcl结果的数据，这里只是和订阅发布结合看看效果

结果：