ROS 话题发布

最新推荐文章于 2024-09-12 19:02:10 发布

My.科研小菜鸡

最新推荐文章于 2024-09-12 19:02:10 发布

阅读量553

点赞数

分类专栏： # ROS 文章标签：人工智能

本文链接：https://blog.csdn.net/qq_41861406/article/details/124873578

版权

ROS 专栏收录该内容

6 篇文章 0 订阅

订阅专栏

//!step1.发布话题所增加的头文件

#include<tf/transform_broadcaster.h>
#include "../../../include/Converter.h"//转换工具
#include <nav_msgs/Path.h>//添加消息类型对应的头问件
#include <nav_msgs/Odometry.h>
#include <std_msgs/Header.h>

//!step2    定义publisher
ros::Publisher stereo_path_pub;

//!step3   设置发布的话题名，和队列长度
stereo_path_pub = nh.advertise<nav_msgs::Odometry>("odometry", 1000);

//！step4  发布消息设置
 std_msgs::Header header;
  header.frame_id = "world";
  header.stamp = ros::Time(cv_ptrLeft->header.stamp.toSec());

 nav_msgs::Odometry odometry;
 odometry.header = header;
 odometry.header.frame_id = "world";
  odometry.child_frame_id = "world";
    
 odometry.pose.pose.position.x = twc.at<float>(0,0);
 odometry.pose.pose.position.y = twc.at<float>(1,0);
  odometry.pose.pose.position.z = twc.at<float>(2,0);
  odometry.pose.pose.orientation.x =q[0];
  odometry.pose.pose.orientation.y =q[1];
   odometry.pose.pose.orientation.z = q[2];
   odometry.pose.pose.orientation.w =q[3];

  stereo_path_pub.publish(odometry);