Landmark 发布

最新推荐文章于 2024-09-26 16:58:09 发布

可爱橘猫咪

最新推荐文章于 2024-09-26 16:58:09 发布

阅读量117

点赞数

文章标签：计算机视觉

本文链接：https://blog.csdn.net/weixin_43869428/article/details/129434353

版权

#include <ros/ros.h>
#include <cartographer_ros_msgs/LandmarkList.h>
#include <cartographer_ros_msgs/LandmarkEntry.h>
#include <nav_msgs/Odometry.h>
#include "std_msgs/String.h"
#include <vector>
#include <unistd.h>
#include <thread>
using namespace std;


cartographer_ros_msgs::LandmarkList landmarkList;
ros::Publisher pub_;
ros::Subscriber sub_;

  void callback(const nav_msgs::Odometry & msg)
{ 
  static int count=0;
  static int huge=0;
  //landmarkList.landmarks.clear();
  landmarkList.header.stamp = ros::Time::now();
  landmarkList.header.frame_id = "map";
  landmarkList.landmarks.resize(100);
  landmarkList.landmarks[count].id = std::to_string(count+huge);//msg.header.seq
  landmarkList.landmarks[count].tracking_from_landmark_transform.position.x = msg.pose.pose.position.x;
  landmarkList.landmarks[count].tracking_from_landmark_transform.position.y = msg.pose.pose.position.y-0.4;
  landmarkList.landmarks[count].tracking_from_landmark_transform.position.z = msg.pose.pose.position.z;
  landmarkList.landmarks[count].tracking_from_landmark_transform.orientation.w = 1;
  landmarkList.landmarks[count].tracking_from_landmark_transform.orientation.x = 0;
  landmarkList.landmarks[count].tracking_from_landmark_transform.orientation.y = 0;
  landmarkList.landmarks[count].tracking_from_landmark_transform.orientation.z = 0;
  landmarkList.landmarks[count].translation_weight = 10000;//设置为0即关闭开关，这个参数很关键！！
  landmarkList.landmarks[count].rotation_weight = 0;  
   cout<<"1111111111111"<<endl;
   count++;
   if(count == 100){
   pub_.publish(landmarkList);
   count = 0;
   huge = huge+16;
   //landmarkList.landmarks.clear();
   }
   
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "subscribe_and_publish");
  ros::NodeHandle n_; 

//  while (ros::ok()){
  sub_ = n_.subscribe("/marker_pose", 10000, callback);
  pub_ = n_.advertise<cartographer_ros_msgs::LandmarkList>("/landmark", 1000);
  
  //sleep(5);
  //ros::spinOnce();
  ros::spin();
   //loop_rate.sleep(); //配合执行频率，sleep一段时间，然后进入下一个循环。
//}
  
  //ros::spin();
  return 0;
}