bool yarp::dev::FrameTransformClient::transformPose(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Vector &input_pose, yarp::sig::Vector &transformed_pose)
{
if (input_pose.size() != 6)
{
yError() << "sorry.. only 6 dimensional vector (3 axes + roll pith and yaw) allowed, dear friend of mine..";
return false;
}
if (transformed_pose.size() != 6)
{
yWarning("FrameTransformClient::transformPose() performance warning: size transformed_pose should be 6, resizing");
transformed_pose.resize(6, 0.0);
}
yarp::sig::Matrix m(4, 4);
if (!getTransform(target_frame_id, source_frame_id, m))
{
yError() << "no transform found between source '" << target_frame_id << "' and target '" << source_frame_id << "'";
return false;
}
FrameTransform t;
t.transFromVec(input_pose[0], input_pose[1], input_pose[2]);
t.rotFromRPY(input_pose[3], input_pose[4], input_pose[5]);
t.fromMatrix(m * t.toMatrix());
transformed_pose[0] = t.translation.tX;
transformed_pose[1] = t.translation.tY;
transformed_pose[2] = t.translation.tZ;
yarp::sig::Vector rot;
rot = t.getRPYRot();
transformed_pose[3] = rot[0];
transformed_pose[4] = rot[1];
transformed_pose[5] = rot[2];
return true;
}
bool yarp::dev::FrameTransformClient::setTransformStatic(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::sig::Matrix &transform)
{
if(target_frame_id == source_frame_id)
{
yError() << "FrameTransformClient::setTransformStatic() Invalid transform detected.\n" \
"\t Source frame and target frame are both equal to " << source_frame_id;
return false;
}
if (canTransform(target_frame_id, source_frame_id))
{
yError() << "FrameTransformClient::setTransform() such static transform already exist, directly or by chaining transforms";
return false;
}
yarp::os::Bottle b;
yarp::os::Bottle resp;
FrameTransform tf;
if (!tf.fromMatrix(transform))
{
yError() << "FrameTransformClient::setTransform() wrong matrix format, it has to be 4 by 4";
return false;
}
b.addVocab(VOCAB_ITRANSFORM);
b.addVocab(VOCAB_TRANSFORM_SET);
b.addString(source_frame_id);
b.addString(target_frame_id);
b.addDouble(-1);
b.addDouble(tf.translation.tX);
b.addDouble(tf.translation.tY);
b.addDouble(tf.translation.tZ);
b.addDouble(tf.rotation.w());
b.addDouble(tf.rotation.x());
b.addDouble(tf.rotation.y());
b.addDouble(tf.rotation.z());
bool ret = m_rpc_InterfaceToServer.write(b, resp);
if (ret)
{
if (resp.get(0).asVocab() != VOCAB_OK)
{
yError() << "FrameTransformClient::setTransform() recived error from server on creating frame between " + source_frame_id + " and " + target_frame_id;
return false;
}
}
else
{
yError() << "FrameTransformClient::setTransform() error on writing on rpc port";
return false;
}
return true;
}
bool FrameTransformServer::parseStartingTf(yarp::os::Searchable &config)
{
if (config.check("USER_TF"))
{
Bottle group = config.findGroup("USER_TF").tail();
for (size_t i = 0; i < group.size(); i++)
{
string tfName;
FrameTransform t;
Bottle b;
Bottle* list = group.get(i).asList();
if(!list)
{
yError() << "no entries in USER_TF group";
return false;
}
tfName = list->get(0).asString();
b = group.findGroup(tfName).tail();
string s = b.toString();
if(b.size() == 18)
{
bool r(true);
Matrix m(4, 4);
for(int i = 0; i < 16; i++)
{
if(!b.get(i).isFloat64())
{
yError() << "transformServer: param " << tfName << ", element " << i << " is not a double.";
r = false;
}
else
{
m.data()[i] = b.get(i).asFloat64();
}
}
if(!b.get(16).isString() || !b.get(17).isString())
{
r = false;
}
if(!r)
{
yError() << "transformServer: param" << tfName << "not correct.. for the 4x4 matrix mode" <<
"you must provide 18 parameter. the matrix, the source frame(string) and the destination frame(string)";
return false;
}
t.fromMatrix(m);
t.src_frame_id = b.get(16).asString();
t.dst_frame_id = b.get(17).asString();
}
else if( b.size() == 8 &&
b.get(0).isFloat64() &&
b.get(1).isFloat64() &&
b.get(2).isFloat64() &&
b.get(3).isFloat64() &&
b.get(4).isFloat64() &&
b.get(5).isFloat64() &&
b.get(6).isString() &&
b.get(7).isString())
{
t.translation.set(b.get(0).asFloat64(), b.get(1).asFloat64(), b.get(2).asFloat64());
t.rotFromRPY(b.get(3).asFloat64(), b.get(4).asFloat64(), b.get(5).asFloat64());
t.src_frame_id = b.get(6).asString();
t.dst_frame_id = b.get(7).asString();
}
else
{
yError() << "transformServer: param" << tfName << "not correct.. a tf requires 8 param in the format:" <<
"x(dbl) y(dbl) z(dbl) r(dbl) p(dbl) y(dbl) src(str) dst(str)";
return false;
}
if(m_yarp_static_transform_storage->set_transform(t))
{
yInfo() << tfName << "from" << t.src_frame_id << "to" << t.dst_frame_id << "succesfully set";
}
}
return true;
}
else
{
yInfo() << "transformServer: no starting tf found";
}
return true;
}
