ROS话题与ROS_INFO输出相机外参格式不一致的代码修改求助

你的核心问题是：用std_msgs/String发布YAML格式字符串时，rostopic echo会以转义特殊字符（如\n、\t）的方式显示内容，而ROS_INFO直接在终端解析这些格式，导致两者显示效果不一致。此外代码存在两处编译错误需先修复。

1. 修正编译错误 #

• 未声明的pose数组：代码中使用pose[i]但未定义该数组，可根据YAML节点中pose的长度声明向量：

  std::vector<double> pose(params_node["pose"].size());
  

• 无效注释：代码中的单独/是无效注释，需直接删除。

2. 统一YAML字符串格式 #

YAML::Node的operator<<会生成带换行、缩进的标准YAML字符串，但rostopic echo默认会转义特殊字符。要让话题显示效果和ROS_INFO一致，有两种可行方案：

方案一：修改rostopic echo显示参数 #

直接使用以下命令查看话题，它会输出原始字符串内容，不转义特殊字符：

rostopic echo /extrinsic --no-escape

方案二：代码中显式生成标准YAML字符串 #

改用YAML::Dump函数替代stringstream输出，确保生成的字符串和原YAML文件格式完全一致：

// 替换原stringstream相关代码
std::string yaml_str = YAML::Dump(params_node);
msg.data = yaml_str;

3. 完整修正代码 #

#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <tf/transform_broadcaster.h>
#include <fstream>
#include <unistd.h>
#include <sys/types.h>
#include <pwd.h>
#include "std_msgs/String.h"
#include "std_msgs/Float64.h"

#include <sstream>
#include <vector>  // 新增头文件用于定义向量

struct passwd *pw = getpwuid(getuid()); // 获取用户主目录
std::string home_dir(pw->pw_dir);

int main(int argc, char** argv) {
  ros::init(argc, argv, "calibration_publisher");
  ros::NodeHandle n("~");
  YAML::Node params_node;
  std::string calibration_file, camera_frame;
  
  
  calibration_file = home_dir + "/.ros/camera_info" + ros::names::parentNamespace(n.getNamespace()) + "_extrinsics.yaml";
  std::cout << "Namespace " << ros::names::parentNamespace(n.getNamespace()) << std::endl;
  std::cout << "Calibration file "<< calibration_file << std::endl;

  params_node = YAML::LoadFile(calibration_file);
  
  if (!n.getParam("camera_frame", camera_frame))
  {
    ROS_ERROR("No camera frame parameter was passed to the calibration publisher");
    return -1;
  }
  
  // 声明pose向量存储外参数据
  std::vector<double> pose(params_node["pose"].size());
  for (auto i = 0; i < params_node["pose"].size(); i++)
  {
    pose[i] = params_node["pose"][i].as<double>();
  }
  
  std::cout << "yaml: " << params_node << std::endl;
  
  ros::Publisher chatter_pub = n.advertise<std_msgs::String>("extrinsic", 10);
  ros::Rate loop_rate(10);
  
  int count = 0;
  while (ros::ok())
  { 
     
    std_msgs::String msg;

    // 用YAML::Dump生成标准格式化字符串
    std::string yaml_str = YAML::Dump(params_node);
    msg.data = yaml_str;

    ROS_INFO("%s", msg.data.c_str());

    chatter_pub.publish(msg);
 
    ros::spinOnce();

    loop_rate.sleep();
    ++count;
  }     
  
  return 0;
}

1. 编译修正后的代码并启动节点
2. 确认ROS_INFO输出格式与原YAML文件一致
3. 执行rostopic echo /extrinsic --no-escape查看话题，显示格式会和ROS_INFO完全匹配

内容的提问来源于stack exchange，提问作者MManuel