/*
parameter format is:
polygon_array: [[[0, 0, 0], [0, 0, 1], [1, 0, 0]], ...]
*/
bool StaticPolygonArrayPublisher::readPolygonArray(const std::string& param_name)
{
if (pnh_->hasParam(param_name)) {
XmlRpc::XmlRpcValue v;
pnh_->param(param_name, v, v);
if (v.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for (size_t toplevel_i = 0; toplevel_i < v.size(); toplevel_i++) { // polygons
XmlRpc::XmlRpcValue polygon_v = v[toplevel_i];
geometry_msgs::PolygonStamped polygon;
if (polygon_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
polygon_v.size() >= 3) {
for (size_t secondlevel_i = 0; secondlevel_i < polygon_v.size(); secondlevel_i++) { // each polygon, vertices
XmlRpc::XmlRpcValue vertex_v = polygon_v[secondlevel_i];
if (vertex_v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
vertex_v.size() == 3 ) { // vertex_v := [x, y, z]
double x = getXMLDoubleValue(vertex_v[0]);
double y = getXMLDoubleValue(vertex_v[1]);
double z = getXMLDoubleValue(vertex_v[2]);
geometry_msgs::Point32 point;
point.x = x;
point.y = y;
point.z = z;
polygon.polygon.points.push_back(point);
}
else {
JSK_NODELET_FATAL("%s[%lu][%lu] is not array or the length is not 3",
param_name.c_str(), toplevel_i, secondlevel_i);
return false;
}
}
polygons_.polygons.push_back(polygon);
// estimate model coefficients
coefficients_.coefficients.push_back(polygonToModelCoefficients(polygon));
}
else {
JSK_NODELET_FATAL("%s[%lu] is not array or not enough points", param_name.c_str(), toplevel_i);
return false;
}
}
return true;
}
else {
JSK_NODELET_FATAL("%s is not array", param_name.c_str());
return false;
}
}
else {
JSK_NODELET_FATAL("no %s is available on parameter server", param_name.c_str());
return false;
}
return true;
}
void FootstepMarker::readPoseParam(ros::NodeHandle& pnh, const std::string param,
tf::Transform& offset) {
XmlRpc::XmlRpcValue v;
geometry_msgs::Pose pose;
if (pnh.hasParam(param)) {
pnh.param(param, v, v);
// check if v is 7 length Array
if (v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
v.size() == 7) {
// safe parameter access by getXMLDoubleValue
pose.position.x = getXMLDoubleValue(v[0]);
pose.position.y = getXMLDoubleValue(v[1]);
pose.position.z = getXMLDoubleValue(v[2]);
pose.orientation.x = getXMLDoubleValue(v[3]);
pose.orientation.y = getXMLDoubleValue(v[4]);
pose.orientation.z = getXMLDoubleValue(v[5]);
pose.orientation.w = getXMLDoubleValue(v[6]);
// converst the message as following: msg -> eigen -> tf
//void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
Eigen::Affine3d e;
tf::poseMsgToEigen(pose, e); // msg -> eigen
tf::transformEigenToTF(e, offset); // eigen -> tf
}
else {
ROS_ERROR_STREAM(param << " is malformed, which should be 7 length array");
}
}
else {
ROS_WARN_STREAM("there is no parameter on " << param);
}
}