admin管理员组

文章数量:1660208

//
// 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();
}

本文标签： PoserosestimateGoalNav

版权声明：本文标题：ROS 2D Pose Estimate,2D Nav Goal 内容由热心网友自发贡献，该文观点仅代表作者本人， 转载请联系作者并注明出处：https://m.elefans.com/dianzi/1729849659a1215243.html， 本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如发现本站有涉嫌抄袭侵权/违法违规的内容，一经查实，本站将立刻删除。