ROS C++ 从参数服务器读取复杂配置
ROS C++ 从参数服务器读取复杂配置
起因: 要从yaml文件中读取一组类似下面这样的数据, python做很简单, 但是要求做成cpp的, 费了好大劲, 调了一上午bug, 主要就是在对 XmlRpc::XmlRpcValue以及static_cast这两个东西不熟悉, 而且bug不明显, 只是会报错Type Error
mode0:
polygon:
- [-1, -0.6]
- [-1, 0.6]
- [0, 0.6]
- [0.5, 0]
- [0, -0.6]
mode1:
polygon:
- [-1, -0.6]
- [-1, 0.6]
- [0, 0.6]
- [0.5, 0]
- [0, -0.6]
首先是python版本的, 一行就解决了! 存在多个类似结构, 也只需要添加循环就好了
self.polygon0 = rospy.get_param('~mode0/polygon', [(-1, -0.6), (-1, 0.6), (0, 0.6), (0.5, 0), (0, -0.6)])
之后是cpp版本, 需要读取11个mode的多边形, 因此需要进行循环赋值
// TODO: 修改polygon的个数为参数配置而不是固定0~10
void initParams() {
std::string node_name = ros::this_node::getName();
XmlRpc::XmlRpcValue temp_polygon;
// 这是polygons的类型
std::vector<std::vector<std::pair<float, float>>> polygons;
for (int i = 0; i <= 10; ++i) {
std::string param_name = node_name + "/mode" + std::to_string(i) + "/polygon";
ROS_DEBUG("Reading parameter: %s", param_name.c_str());
if (nh.getParam(param_name, temp_polygon) && temp_polygon.getType() == XmlRpc::XmlRpcValue::TypeArray) {
// 先创建一个polygons[i], 后面才能在这个数据中进行赋值
polygons.push_back(std::vector<std::pair<float, float>>());
try {
for (int j = 0; j < temp_polygon.size(); ++j) {
XmlRpc::XmlRpcValue point = temp_polygon[j];
float x,y;
if (point.getType() == XmlRpc::XmlRpcValue::TypeArray && point.size() == 2) {
// 第一个卡点就是在这里, 最开始我想用static_cast(point[0])直接去读double的数据,期待他做隐式类型转换, 结果他完全不做,直接抛异常, type error, 这里面判断类型的又很多, 因此问题定位花了好久
if (point[0].getType() == XmlRpc::XmlRpcValue::TypeInt) {
// 如果 point[0] 是整型，则先转换为 int，再转换为 double
x = static_cast<int>(point[0]);
ROS_DEBUG("point%d[0] INT x=%f",i,x);
} else if (point[0].getType() == XmlRpc::XmlRpcValue::TypeDouble) {
// 如果 point[0] 已经是 double 类型，则直接转换
x = static_cast<double>(point[0]);
ROS_DEBUG("point%d[0] FLOAT x=%f",i,x);
} else {
ROS_ERROR("Invalid X coordinate type for parameter: %s", param_name.c_str());
continue;
}
if (point[1].getType() == XmlRpc::XmlRpcValue::TypeInt) {
y = static_cast<int>(point[1]);
ROS_DEBUG("point%d[0] INT y=%f",i,y);
} else if (point[1].getType() == XmlRpc::XmlRpcValue::TypeDouble) {
y = static_cast<double>(point[1]);
ROS_DEBUG("point%d[0] FLOAT y=%f",i,y);
} else {
ROS_ERROR("Invalid Y coordinate type for parameter: %s", param_name.c_str());
continue;
}
// 添加一个多边形
polygons[i].push_back(std::make_pair(x, y));
}else {
ROS_ERROR("Invalid polygon point format for parameter: %s", param_name.c_str());
}
}
} catch (const XmlRpc::XmlRpcException& ex) {
ROS_ERROR("Error parsing polygon data: %s", ex.getMessage().c_str());
}
} else {
ROS_ERROR("Failed to read parameter: %s", param_name.c_str());
}
ROS_INFO("Polygon %d loaded successfully!",i);
}
ROS_INFO("%lu polygons added!",polygons.size());
}
// DEBUG 用于测试收到的yaml文件中数据的类型, 可以用ROS_INFO() 输出收到的类型
// std::string xmlRpcValueTypeToString(XmlRpc::XmlRpcValue::Type type) {
// switch (type) {
// case XmlRpc::XmlRpcValue::TypeInvalid: return "Invalid";
// case XmlRpc::XmlRpcValue::TypeBoolean: return "Boolean";
// case XmlRpc::XmlRpcValue::TypeInt: return "Int";
// case XmlRpc::XmlRpcValue::TypeDouble: return "Double";
// case XmlRpc::XmlRpcValue::TypeString: return "String";
// case XmlRpc::XmlRpcValue::TypeDateTime: return "DateTime";
// case XmlRpc::XmlRpcValue::TypeBase64: return "Base64";
// case XmlRpc::XmlRpcValue::TypeArray: return "Array";
// case XmlRpc::XmlRpcValue::TypeStruct: return "Struct";
// default: return "Unknown";
// }
// }
// DEBUG 用于输出收到的yaml文件的数据
// std::string printXmlRpcValue(const XmlRpc::XmlRpcValue &value) {
// std::ostringstream stream;
// switch (value.getType()) {
// case XmlRpc::XmlRpcValue::TypeInt:
// stream << static_cast(value);
// break;
// case XmlRpc::XmlRpcValue::TypeDouble:
// stream << static_cast(value);
// break;
// case XmlRpc::XmlRpcValue::TypeBoolean:
// stream << (static_cast(value) ? "true" : "false");
// break;
// case XmlRpc::XmlRpcValue::TypeString:
// stream << static_cast(value);
// break;
// case XmlRpc::XmlRpcValue::TypeArray:
// stream << "[";
// for (int i = 0; i < value.size(); ++i) {
// if (i > 0) {
// stream << ", ";
// }
// stream << printXmlRpcValue(value[i]);
// }
// stream << "]";
// break;
// case XmlRpc::XmlRpcValue::TypeStruct:
// stream << "{";
// for (XmlRpc::XmlRpcValue::const_iterator it = value.begin(); it != value.end(); ++it) {
// if (it != value.begin()) {
// stream << ", ";
// }
// stream << it->first << ": " << printXmlRpcValue(it->second);
// }
// stream << "}";
// break;
// default:
// stream << "Unknown Type";
// break;
// }
// return stream.str();
// }