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 yarp::dev::FrameTransformClient::transformQuaternion(const std::string &target_frame_id, const std::string &source_frame_id, const yarp::math::Quaternion &input_quaternion, yarp::math::Quaternion &transformed_quaternion)
{
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.rotation=input_quaternion;
transformed_quaternion.fromRotationMatrix(m * t.toMatrix());
return true;
}
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 FrameTransformServer::read(yarp::os::ConnectionReader& connection)
{
LockGuard lock(m_mutex);
yarp::os::Bottle in;
yarp::os::Bottle out;
bool ok = in.read(connection);
if (!ok) return false;
string request = in.get(0).asString();
// parse in, prepare out
int code = in.get(0).asVocab();
bool ret = false;
if (code == VOCAB_ITRANSFORM)
{
int cmd = in.get(1).asVocab();
if (cmd == VOCAB_TRANSFORM_SET)
{
if (in.size() != 12)
{
yError() << "FrameTransformServer::read() protocol error";
out.clear();
out.addVocab(VOCAB_FAILED);
}
else
{
FrameTransform t;
t.src_frame_id = in.get(2).asString();
t.dst_frame_id = in.get(3).asString();
double duration = in.get(4).asDouble();
t.translation.tX = in.get(5).asDouble();
t.translation.tY = in.get(6).asDouble();
t.translation.tZ = in.get(7).asDouble();
t.rotation.w() = in.get(8).asDouble();
t.rotation.x() = in.get(9).asDouble();
t.rotation.y() = in.get(10).asDouble();
t.rotation.z() = in.get(11).asDouble();
t.timestamp = yarp::os::Time::now();
bool static_transform;
if (duration > 0)
{
static_transform = false;
}
else
{
static_transform = true;
}
if (static_transform)
{
ret = m_yarp_static_transform_storage->set_transform(t);
}
else
{
ret = m_yarp_timed_transform_storage->set_transform(t);
}
if (ret == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
else
{
out.clear();
out.addVocab(VOCAB_FAILED);
yError() << "FrameTransformServer::read() something strange happened";
}
}
}
else if (cmd == VOCAB_TRANSFORM_DELETE_ALL)
{
m_yarp_timed_transform_storage->clear();
m_yarp_static_transform_storage->clear();
m_ros_timed_transform_storage->clear();
m_ros_static_transform_storage->clear();
out.clear();
out.addVocab(VOCAB_OK);
}
else if (cmd == VOCAB_TRANSFORM_DELETE)
{
string frame1 = in.get(2).asString();
string frame2 = in.get(3).asString();
bool ret1 = m_yarp_timed_transform_storage->delete_transform(frame1, frame2);
if (ret1 == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
else
{
bool ret2 = m_yarp_static_transform_storage->delete_transform(frame1, frame2);
if (ret2 == true)
{
out.clear();
out.addVocab(VOCAB_OK);
}
}
}
else
{
yError("Invalid vocab received in FrameTransformServer");
out.clear();
out.addVocab(VOCAB_ERR);
}
}
else if(request == "help")
{
out.addVocab(Vocab::encode("many"));
out.addString("'list': get all transforms stored");
out.addString("'delete_all': delete all transforms");
out.addString("'set_static_transform <src> <dst> <x> <y> <z> <roll> <pitch> <yaw>': create a static transform");
}
else if (request == "set_static_transform")
{
FrameTransform t;
t.src_frame_id = in.get(1).asString();
t.dst_frame_id = in.get(2).asString();
t.translation.tX = in.get(3).asDouble();
t.translation.tY = in.get(4).asDouble();
t.translation.tZ = in.get(5).asDouble();
t.rotFromRPY(in.get(6).asDouble(), in.get(7).asDouble(), in.get(8).asDouble());
t.timestamp = yarp::os::Time::now();
bool static_transform = true;
ret = m_yarp_static_transform_storage->set_transform(t);
if (ret == true)
{
yInfo() << "set_static_transform done";
out.addString("set_static_transform done");
}
else
{
yError() << "FrameTransformServer::read() something strange happened";
}
}
else if(request == "delete_all")
{
m_yarp_timed_transform_storage->clear();
m_yarp_static_transform_storage->clear();
m_ros_timed_transform_storage->clear();
m_ros_static_transform_storage->clear();
yInfo() << "delete_all done";
out.addString("delete_all done");
}
else if (request == "list")
{
out.addVocab(Vocab::encode("many"));
list_response(out);
}
else
{
yError("Invalid vocab received in FrameTransformServer");
out.clear();
out.addVocab(VOCAB_ERR);
}
yarp::os::ConnectionWriter *returnToSender = connection.getWriter();
if (returnToSender != NULL)
{
out.write(*returnToSender);
}
else
{
yError() << "FrameTransformServer: invalid return to sender";
}
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;
}
