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; }