rviz里显示的一般是nav_msgs::path,而evo一般评估nav_msgs::odometry,但是实际上二者发布的都是位姿的优化后的结果

 将odometry转成path:

1、将odom赋值给geometry_msgs::PoseStamped 类型的变量

2、path.poses.push_back(pose_stamped_this),其中path为nav_msgs::path,pose_stamped_this为geometry_msgs::PoseStamped

3、最后把path发布,即可在rviz 中显示

#include<stdio.h>
#include<ros/ros.h>
#include<nav_msgs/Path.h>
#include<nav_msgs/Odometry.h>

ros::Publisher path_pub;    //发布path的节点
ros::Subscriber odom_sub;   //接收odometry的节点

nav_msgs::Path path;        //发布的路径
bool init_flag=1;             //path初始化标记位

void odomCallback(const nav_msgs::Odometry::ConstPtr& odom){
    if(init_flag){
        path.header=odom->header;
        init_flag=0;
    }

    geometry_msgs::PoseStamped pose_stamped_this;
    pose_stamped_this.header=odom->header;
    pose_stamped_this.pose=odom->pose.pose;

    path.poses.push_back(pose_stamped_this);
    path_pub.publish(path);
}


int main (int argc, char **argv)
{
    ros::init (argc, argv, "odometry2path");
    ros::NodeHandle nh;
    
 //       记得在此处更改odometry消息topic名称     //
    odom_sub  = nh.subscribe<nav_msgs::Odometry>("/camera/odom/sample", 10, odomCallback);
    path_pub = nh.advertise<nav_msgs::Path>("/path",10, true);
 

    
    ros::spin();
    // ros::Rate loop_rate(50);
    // loop_rate.sleep();
    return 0;
}

参考:

https://blog.csdn.net/T_T___T_T/article/details/131010917

  • 3
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值