/*
   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);
  }
}