26 throw ::std::runtime_error(::
cras::format(
"Cannot load %s parameter from an array of length %lu",
27 "tf2::Vector3", v.size()));
28 return ::tf2::Vector3(v[0], v[1], v[2]);
32 if (v.size() != 3 && v.size() != 4)
33 throw ::std::runtime_error(::
cras::format(
"Cannot load %s parameter from an array of length %lu",
34 "tf2::Quaternion", v.size()));
38 m.setX(v[0]); m.setY(v[1]); m.setZ(v[2]); m.setW(v[3]);
42 m.
setRPY(v[0], v[1], v[2]);
48 if (v.size() != 6 && v.size() != 7 && v.size() != 16)
49 throw ::std::runtime_error(::
cras::format(
"Cannot load %s parameter from an array of length %lu",
50 "tf2::Transform", v.size()));
52 if (v.size() == 6 || v.size() == 7)
68 m.
setBasis({v[0], v[1], v[2], v[4], v[5], v[6], v[8], v[9], v[10]});
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
inline ::std::string format(const char *format, ::va_list args)
DEFINE_CONVERTING_GET_PARAM(::Eigen::Vector3d, ::std::vector< double >, "", [](const ::std::vector< double > &v){ checkSize(v, 3, "vector");return ::Eigen::Vector3d(v.data());}) DEFINE_CONVERTING_GET_PARAM(