ROS 2D Pose Estimate,2D Nav Goal

最新推荐文章于 2025-03-31 10:18:49 发布

海洋2416

最新推荐文章于 2025-03-31 10:18:49 发布

阅读量1.5k

点赞数

分类专栏： ROS 文章标签：自动驾驶 linux

本文链接：https://blog.csdn.net/qq_38966867/article/details/120542588

版权

ROS 专栏收录该内容

12 篇文章

订阅专栏

在这里插入图片描述

//
// Created by hhy on 2021/9/29.
//

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <ros/ros.h>

// PoseWithCovarianceStamped
void GetPoseEstimate(
    const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &initial) {
  float x = initial->pose.pose.position.x;
  float y = initial->pose.pose.position.y;
  float z = initial->pose.pose.position.z;
  ROS_INFO_STREAM("GetPoseEstimate   x:" << x << " y:" << y << " z:" << z);
}

void GetNavGoal(const geometry_msgs::PoseStamped::ConstPtr &end) {
  float x = end->pose.position.x;
  float y = end->pose.position.y;
  float z = end->pose.position.z;
  ROS_INFO_STREAM("GetNavGoal   x:" << x << " y:" << y << " z:" << z);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "sub_point");
  ros::NodeHandle nh;
  auto sub_start = nh.subscribe("/initialpose", 1, GetPoseEstimate);
  auto sub_goal = nh.subscribe("/move_base_simple/goal", 1,GetNavGoal);
  ROS_INFO_STREAM("spin");
  ros::spin();
}