void TransformROSBridge::calculateTwist(const tf::Transform& _current_trans, const tf::Transform& _prev_trans, tf::Vector3& _linear_twist, tf::Vector3& _angular_twist, double _dt)
{
// current rotation matrix
tf::Matrix3x3 current_basis = _current_trans.getBasis();
// linear twist
tf::Vector3 current_origin = _current_trans.getOrigin();
tf::Vector3 prev_origin = _prev_trans.getOrigin();
_linear_twist = current_basis.transpose() * ((current_origin - prev_origin) / _dt);
// angular twist
// R = exp(omega_w*dt) * prev_R
// omega_w is described in global coordinates in relationships of twist transformation.
// it is easier to calculate omega using hrp functions than tf functions
tf::Matrix3x3 prev_basis = _prev_trans.getBasis();
double current_rpy[3], prev_rpy[3];
current_basis.getRPY(current_rpy[0], current_rpy[1], current_rpy[2]);
prev_basis.getRPY(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Matrix33 current_hrpR = hrp::rotFromRpy(current_rpy[0], current_rpy[1], current_rpy[2]);
hrp::Matrix33 prev_hrpR = hrp::rotFromRpy(prev_rpy[0], prev_rpy[1], prev_rpy[2]);
hrp::Vector3 hrp_omega = current_hrpR.transpose() * hrp::omegaFromRot(current_hrpR * prev_hrpR.transpose()) / _dt;
_angular_twist.setX(hrp_omega[0]);
_angular_twist.setY(hrp_omega[1]);
_angular_twist.setZ(hrp_omega[2]);
return;
}
void MarkerVis::printTransform(tf::Transform T)
{
printf("[ %f %f %f %f \n %f %f %f %f \n %f %f %f %f \n %f %f %f %f]\n", T.getBasis()[0][0], T.getBasis()[0][1], T.getBasis()[0][2], T.getBasis()[0][3],
T.getBasis()[1][0], T.getBasis()[1][1], T.getBasis()[1][2], T.getBasis()[1][3],
T.getBasis()[2][0], T.getBasis()[2][1], T.getBasis()[2][2], T.getBasis()[2][3],
T.getBasis()[3][0], T.getBasis()[3][1], T.getBasis()[3][2], T.getBasis()[3][3]);
}
bool KlamptToROS(const RigidTransform& kT,tf::Transform& T)
{
T.getOrigin().setValue(kT.t.x,kT.t.y,kT.t.z);
Vector3 row;
kT.R.getRow1(row);
T.getBasis()[0].setValue(row.x,row.y,row.z);
kT.R.getRow2(row);
T.getBasis()[1].setValue(row.x,row.y,row.z);
kT.R.getRow3(row);
T.getBasis()[2].setValue(row.x,row.y,row.z);
return true;
}
bool tfGreaterThan(const tf::Transform& tf, double dist, double angle)
{
double d = tf.getOrigin().length();
if (d >= dist) return true;
double trace = tf.getBasis()[0][0] + tf.getBasis()[1][1] + tf.getBasis()[2][2];
double a = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0)));
if (a > angle) return true;
return false;
}
MultiAgent3dNavigation::MultiAgent3dNavigation(const tf::Transform& world_to_cam, const tf::Transform& drone_to_marker, const tf::Transform& drone_to_front_cam, const ranav::TParam &p)
{
this->params = p;
motionModel = new ranav::Rotor3dMotionModel();
motionModel->init(p);
ekf = new ranav::EKF(motionModel);
ekf->init(p);
ttc = new ranav::TargetTrackingController();
ttc->init(p);
addOn3d.resize(motionModel->getNumAgents());
this->world_to_cam = world_to_cam;
this->drone_to_marker = drone_to_marker;
this->drone_to_front_cam = drone_to_front_cam;
double roll, pitch, yaw;
world_to_cam.getBasis().getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d world_to_cam2d(world_to_cam.getOrigin().getX(),world_to_cam.getOrigin().getY(), yaw);
world_to_cam2d += Eigen::Vector3d(2, 0, 0); // HACK: move virtual 2D camera pose of tilted camera to have a circular field of view around (0, 0)
tf::Quaternion q;
q.setRPY(0, 0, world_to_cam2d(2));
world_to_cam_2d = tf::Transform(q, tf::Vector3(world_to_cam2d(0), world_to_cam2d(1), 0));
drone_to_marker.getBasis().getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d drone_to_marker2d(drone_to_marker.getOrigin().getX(), drone_to_marker.getOrigin().getY(), yaw);
drone_to_front_cam.getBasis().getEulerYPR(yaw, pitch, roll);
Eigen::Vector3d drone_to_front_cam2d(drone_to_front_cam.getOrigin().getX(), drone_to_front_cam.getOrigin().getY(), yaw);
drone_to_front_cam2d += Eigen::Vector3d(0.8, 0, 0); // HACK: move virtual 2D camera pose of tilted camera to have a circular field of view around (0, 0)
q.setRPY(0, 0, drone_to_front_cam2d(2));
drone_to_front_cam_2d = tf::Transform(q, tf::Vector3(drone_to_front_cam2d(0), drone_to_front_cam2d(1), 0));
ranav::AllModels models;
ranav::Marker3dSensorModel *sm;
sm = new ranav::Marker3dSensorModel(-1, 0);
sm->init(p);
sm->setCameraPose(world_to_cam2d); // HACK: overwrites parameter of config file
sm->setMarkerPose(drone_to_marker2d); // HACK: overwrites parameter of config file
models()[ranav::Index(-1, 0)] = sm;
sm = new ranav::Marker3dSensorModel(0, 1);
sm->init(p);
sm->setCameraPose(drone_to_front_cam2d); // HACK: overwrites ...
models()[ranav::Index(0, 1)] = sm;
ttc->getTopology().setAllSensorModels(models);
sensorModels = ttc->getTopology().getSensorModelsNonconst();
}
void transformTFToKDL(const tf::Transform &t, KDL::Frame &k)
{
for (unsigned int i = 0; i < 3; ++i)
k.p[i] = t.getOrigin()[i];
for (unsigned int i = 0; i < 9; ++i)
k.M.data[i] = t.getBasis()[i/3][i%3];
}
static boost::numeric::ublas::matrix<double> transformAsMatrix(const tf::Transform& bt)
{
boost::numeric::ublas::matrix<double> outMat(4,4);
// double * mat = outMat.Store();
double mv[12];
bt.getBasis().getOpenGLSubMatrix(mv);
btVector3 origin = bt.getOrigin();
outMat(0,0)= mv[0];
outMat(0,1) = mv[4];
outMat(0,2) = mv[8];
outMat(1,0) = mv[1];
outMat(1,1) = mv[5];
outMat(1,2) = mv[9];
outMat(2,0) = mv[2];
outMat(2,1) = mv[6];
outMat(2,2) = mv[10];
outMat(3,0) = outMat(3,1) = outMat(3,2) = 0;
outMat(0,3) = origin.x();
outMat(1,3) = origin.y();
outMat(2,3) = origin.z();
outMat(3,3) = 1;
return outMat;
};
void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform,
const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn,
sensor_msgs::PointCloud & cloudOut) const
{
tf::Vector3 origin = net_transform.getOrigin();
tf::Matrix3x3 basis = net_transform.getBasis();
unsigned int length = cloudIn.points.size();
// Copy relevant data from cloudIn, if needed
if (&cloudIn != &cloudOut)
{
cloudOut.header = cloudIn.header;
cloudOut.points.resize(length);
cloudOut.channels.resize(cloudIn.channels.size());
for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i)
cloudOut.channels[i] = cloudIn.channels[i];
}
// Transform points
cloudOut.header.stamp = target_time;
cloudOut.header.frame_id = target_frame;
for (unsigned int i = 0; i < length ; i++) {
transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]);
}
}
void add_edge(GraphOptimizer3D* optimizer, int id1, int id2, tf::Transform pose,
double sigma_position, double sigma_orientation) {
Vector6 p;
double roll, pitch, yaw;
MAT_to_RPY(pose.getBasis(), roll, pitch, yaw);
p[0] = pose.getOrigin().x();
p[1] = pose.getOrigin().y();
p[2] = pose.getOrigin().z();
p[3] = roll;
p[4] = pitch;
p[5] = yaw;
Matrix6 m = Matrix6::eye(1.0);
double ip = 1 / SQR(sigma_position);
double io = 1 / SQR(sigma_orientation);
m[0][0] = ip;
m[1][1] = ip;
m[2][2] = ip;
m[3][3] = io;
m[4][4] = io;
m[5][5] = io;
GraphOptimizer3D::Vertex* v1 = optimizer->vertex(id1);
GraphOptimizer3D::Vertex* v2 = optimizer->vertex(id2);
if (!v1) {
cerr << "adding edge, id1=" << id1 << " not found" << endl;
}
if (!v2) {
cerr << "adding edge, id2=" << id2 << " not found" << endl;
}
Transformation3 t = Transformation3::fromVector(p);
GraphOptimizer3D::Edge* e = optimizer->addEdge(v1, v2, t, m);
if (!e) {
cerr << "adding edge failed" << endl;
}
}
/** @brief Helper function to convert Eigen transformation to tf */
Eigen::Matrix4f EigenfromTf(tf::Transform trans)
{
Eigen::Matrix4f eignMat;
eignMat(0,3) = trans.getOrigin().getX();
eignMat(1,3) = trans.getOrigin().getY();
eignMat(2,3) = trans.getOrigin().getZ();
for (int i=0;i<3;i++)
{
eignMat(i,0) = trans.getBasis().getRow(i).getX();
eignMat(i,1) = trans.getBasis().getRow(i).getY();
eignMat(i,2) = trans.getBasis().getRow(i).getZ();
}
eignMat(3,3) = 1;
//ROS_INFO("trans: %f, %f, %f %f | %f, %f, %f %f | %f, %f, %f %f", eignMat(0,0), eignMat(0,1), eignMat(0,2), eignMat(0,3), eignMat(1,0), eignMat(1,1), eignMat(1,2), eignMat(1,3), eignMat(2,0), eignMat(2,1), eignMat(2,2), eignMat(2,3));
return eignMat;
}
void Person::update(int cx, int cy, tf::Transform transform, Time time)
{
double dt = time.sec - last_update_time_.sec;
Eigen::Matrix<double, 3, 4> tf_matrix;
//Copy rotation matrix
tf::Matrix3x3 rotation = transform.getBasis();
for(int i=0; i < 3; i++)
{
tf::Vector3 row = rotation.getRow(i);
tf_matrix(i,0) = row.x();
tf_matrix(i,1) = row.y();
tf_matrix(i,2) = row.z();
}
//Copy translation matrix
tf::Vector3 translation = transform.getOrigin();
tf_matrix(0,3) = translation.x();
tf_matrix(1,3) = translation.y();
tf_matrix(2,3) = translation.z();
kf_->predict(dt);
kf_->update(cx, cy, tf_matrix, time);
last_update_time_ = time;
life_time_ = 5;
return;
}
void PathPlanningWidget::pointPosUpdated_slot(const tf::Transform& point_pos, const char* marker_name)
{
/*! When the user updates the position of the Way-Point or the User Interactive Marker, the information in the TreeView also needs to be updated to correspond to the current pose of the InteractiveMarkers.
*/
QAbstractItemModel *model = ui_.treeView->model();
ROS_INFO_STREAM("Updating marker name:"<<marker_name);
tf::Vector3 p = point_pos.getOrigin();
tfScalar rx,ry,rz;
point_pos.getBasis().getRPY(rx,ry,rz,1);
rx = RAD2DEG(rx);
ry = RAD2DEG(ry);
rz = RAD2DEG(rz);
if((strcmp(marker_name,"add_point_button") == 0) || (atoi(marker_name)==0))
{
QString pos_s;
pos_s = QString::number(p.x()) + "; " + QString::number(p.y()) + "; " + QString::number(p.z()) + ";";
QString orient_s;
orient_s = QString::number(rx) + "; " + QString::number(ry) + "; " + QString::number(rz) + ";";
model->setData(model->index(0,0),QVariant("add_point_button"),Qt::EditRole);
model->setData(model->index(0,1),QVariant(pos_s),Qt::EditRole);
model->setData(model->index(0,2),QVariant(orient_s),Qt::EditRole);
}
else
{
int changed_marker = atoi(marker_name);
//**********************update the positions and orientations of the children as well***********************************************************************************************
QModelIndex ind = model->index(changed_marker, 0);
QModelIndex chldind_pos = model->index(0, 0, ind);
QModelIndex chldind_orient = model->index(1, 0, ind);
//set the strings of each axis of the position
QString pos_x = QString::number(p.x());
QString pos_y = QString::number(p.y());
QString pos_z = QString::number(p.z());
//repeat that with the orientation
QString orient_x = QString::number(rx);
QString orient_y = QString::number(ry);
QString orient_z = QString::number(rz);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 1, chldind_pos), QVariant(pos_x), Qt::EditRole);
model->setData(model->index(1, 1, chldind_pos), QVariant(pos_y), Qt::EditRole);
model->setData(model->index(2, 1, chldind_pos), QVariant(pos_z), Qt::EditRole);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 2, chldind_orient), QVariant(orient_x), Qt::EditRole);
model->setData(model->index(1, 2, chldind_orient), QVariant(orient_y), Qt::EditRole);
model->setData(model->index(2, 2, chldind_orient), QVariant(orient_z), Qt::EditRole);
//*****************************************************************************************************************************************************************************************
}
}
/**\fn void print_tf(tf::Transform xf)
* \brief print a tf::Transform object
* \param xf - a tf::Transform object to print
* \return void
* \ingroup Kinematics
*/
void print_tf(tf::Transform xf) {
tf::Matrix3x3 rr = xf.getBasis();
tf::Vector3 vv = xf.getOrigin();
std::stringstream ss;
cout << fixed;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) ss << rr[i][j] << "\t ";
ss << vv[i] << endl;
}
ss << "0\t 0\t 0\t 1\n";
log_msg("\n%s", ss.str().c_str());
}
Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
Eigen::Affine3d e;
for (int i = 0; i < 3; i++) {
e.matrix()(i, 3) = t.getOrigin()[i];
for (int j = 0; j < 3; j++) {
e.matrix()(i, j) = t.getBasis()[i][j];
}
}
// Fill in identity in last row
for (int col = 0; col < 3; col++)
e.matrix()(3, col) = 0;
e.matrix()(3, 3) = 1;
return e;
}
Eigen::Affine3d transformTFToEigen(const tf::Transform &t) {
Eigen::Affine3d e;
// treat the Eigen::Affine as a 4x4 matrix:
for (int i = 0; i < 3; i++) {
e.matrix()(i, 3) = t.getOrigin()[i]; //copy the origin from tf to Eigen
for (int j = 0; j < 3; j++) {
e.matrix()(i, j) = t.getBasis()[i][j]; //and copy 3x3 rotation matrix
}
}
// Fill in identity in last row
for (int col = 0; col < 3; col++)
e.matrix()(3, col) = 0;
e.matrix()(3, 3) = 1;
return e;
}
/* Some helper functions */
void tfTransformToHomogeniousMatrix (const tf::Transform& tfTransform, brics_3d::IHomogeneousMatrix44::IHomogeneousMatrix44Ptr& transformMatrix)
{
double mv[12];
tfTransform.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = tfTransform.getOrigin();
double* matrixPtr = transformMatrix->setRawData();
/* matrices are column-major */
matrixPtr[0] = mv[0]; matrixPtr[4] = mv[4]; matrixPtr[8] = mv[8]; matrixPtr[12] = origin.x();
matrixPtr[1] = mv[1]; matrixPtr[5] = mv[5]; matrixPtr[9] = mv[9]; matrixPtr[13] = origin.y();
matrixPtr[2] = mv[2]; matrixPtr[6] = mv[6]; matrixPtr[10] = mv[10]; matrixPtr[14] = origin.z();
matrixPtr[3] = 1; matrixPtr[7] = 1; matrixPtr[11] = 1; matrixPtr[15] = 1;
}
void transformToEigenMatrix4f(tf::Transform &t, Eigen::Matrix4f &m)
{
tf::Matrix3x3 basis;
tf::Vector3 origin, rot[3];
basis = t.getBasis();
origin = t.getOrigin();
for(int i=0;i<3;i++)
rot[i] = basis.getRow(i);
m << rot[0][0], rot[0][1], rot[0][2], origin.x(),
rot[1][0], rot[1][1], rot[1][2], origin.y(),
rot[2][0], rot[2][1], rot[2][2], origin.z(),
0, 0, 0, 1;
}
bool ROSToKlampt(const tf::Transform& T,RigidTransform& kT)
{
kT.t.set(T.getOrigin().x(),T.getOrigin().y(),T.getOrigin().z());
Vector3 row1(T.getBasis()[0].x(),T.getBasis()[0].y(),T.getBasis()[0].z());
Vector3 row2(T.getBasis()[1].x(),T.getBasis()[1].y(),T.getBasis()[1].z());
Vector3 row3(T.getBasis()[2].x(),T.getBasis()[2].y(),T.getBasis()[2].z());
kT.R.setRow1(row1);
kT.R.setRow2(row2);
kT.R.setRow3(row3);
return true;
}
void
transformAsMatrix (const tf::Transform& bt, Eigen::Matrix4f &out_mat)
{
double mv[12];
bt.getBasis ().getOpenGLSubMatrix (mv);
tf::Vector3 origin = bt.getOrigin ();
out_mat (0, 0) = mv[0]; out_mat (0, 1) = mv[4]; out_mat (0, 2) = mv[8];
out_mat (1, 0) = mv[1]; out_mat (1, 1) = mv[5]; out_mat (1, 2) = mv[9];
out_mat (2, 0) = mv[2]; out_mat (2, 1) = mv[6]; out_mat (2, 2) = mv[10];
out_mat (3, 0) = out_mat (3, 1) = out_mat (3, 2) = 0; out_mat (3, 3) = 1;
out_mat (0, 3) = origin.x ();
out_mat (1, 3) = origin.y ();
out_mat (2, 3) = origin.z ();
}
void add_vertex(GraphOptimizer3D* optimizer, int id, tf::Transform pose) {
Vector6 p;
double roll, pitch, yaw;
MAT_to_RPY(pose.getBasis(), roll, pitch, yaw);
p[0] = pose.getOrigin().x();
p[1] = pose.getOrigin().y();
p[2] = pose.getOrigin().z();
p[3] = roll;
p[4] = pitch;
p[5] = yaw;
Transformation3 t = Transformation3::fromVector(p);
Matrix6 identity = Matrix6::eye(1.0);
GraphOptimizer3D::Vertex* v = optimizer->addVertex(id, t, identity);
if (!v) {
cerr << "adding vertex id=" << id << "failed" << endl;
}
}
void tfToEigenRt(
const tf::Transform& tf,
Matrix3f& R,
Vector3f& t)
{
double mv[12];
tf.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = tf.getOrigin();
R(0, 0) = mv[0]; R(0, 1) = mv[4]; R(0, 2) = mv[8];
R(1, 0) = mv[1]; R(1, 1) = mv[5]; R(1, 2) = mv[9];
R(2, 0) = mv[2]; R(2, 1) = mv[6]; R(2, 2) = mv[10];
t(0, 0) = origin.x();
t(1, 0) = origin.y();
t(2, 0) = origin.z();
}
void OdomTf::printTf(tf::Transform tf) {
tf::Vector3 tfVec;
tf::Matrix3x3 tfR;
tf::Quaternion quat;
tfVec = tf.getOrigin();
cout << "vector from reference frame to child frame: " << tfVec.getX() << "," << tfVec.getY() << "," << tfVec.getZ() << endl;
tfR = tf.getBasis();
cout << "orientation of child frame w/rt reference frame: " << endl;
tfVec = tfR.getRow(0);
cout << tfVec.getX() << "," << tfVec.getY() << "," << tfVec.getZ() << endl;
tfVec = tfR.getRow(1);
cout << tfVec.getX() << "," << tfVec.getY() << "," << tfVec.getZ() << endl;
tfVec = tfR.getRow(2);
cout << tfVec.getX() << "," << tfVec.getY() << "," << tfVec.getZ() << endl;
quat = tf.getRotation();
cout << "quaternion: " << quat.x() << ", " << quat.y() << ", "
<< quat.z() << ", " << quat.w() << endl;
}
bool control(PID_control::controlserver::Request &req,
PID_control::controlserver::Response &ret) {
tf::vector3MsgToTF(req.target_error,sp);
tf::transformMsgToTF(req.transform,pose);
tf::vector3MsgToTF(req.velocity,vel);
if (req.reset) {
pParam=2,iParam=0,dParam=0,vParam=-2,cumul=0,lastE=0,pAlphaE=0,pBetaE=0,psp2=0,psp1=0,prevYaw=0;
ROS_INFO("Controller reset");
}
e = (pose*sp).z() - pose.getOrigin().z();
cumul=cumul+e;
pv=pParam*e;
thrust=5.335+pv+iParam*cumul+dParam*(e-lastE)+vel.z()*vParam;
lastE=e;
// Horizontal control:
vx = pose*tf::Vector3(1,0,0);
vy = pose*tf::Vector3(0,1,0);
alphaE=(vy.z()-pose.getOrigin().z());
alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE);
betaE=(vx.z()-pose.getOrigin().z());
betaCorr=-0.25*betaE-2.1*(betaE-pBetaE);
pAlphaE=alphaE;
pBetaE=betaE;
alphaCorr=alphaCorr+sp.y()*0.005+1*(sp.y()-psp2);
betaCorr=betaCorr-sp.x()*0.005-1*(sp.x()-psp1);
psp2=sp.y();
psp1=sp.x();
pose.getBasis().getRPY(t1, t2, yaw);
rotCorr=yaw*0.1+2*(yaw-prevYaw);
prevYaw = yaw;
// Decide of the motor velocities:
a=thrust*((double)1-alphaCorr+betaCorr+rotCorr);
b=thrust*((double)1-alphaCorr-betaCorr-rotCorr);
c=thrust*((double)1+alphaCorr-betaCorr+rotCorr);
d=thrust*((double)1+alphaCorr+betaCorr-rotCorr);
ret.a=a;
ret.b=b;
ret.c=c;
ret.d=d;
}
AffineTransform eigenAffineFromTf(const tf::Transform& tf)
{
AffineTransform affine;
double mv[12];
tf.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = tf.getOrigin();
affine(0, 0) = mv[0]; affine(0, 1) = mv[4]; affine (0, 2) = mv[8];
affine(1, 0) = mv[1]; affine(1, 1) = mv[5]; affine (1, 2) = mv[9];
affine(2, 0) = mv[2]; affine(2, 1) = mv[6]; affine (2, 2) = mv[10];
affine(3, 0) = affine(3, 1) = affine(3, 2) = 0; affine(3, 3) = 1;
affine(0, 3) = origin.x();
affine(1, 3) = origin.y();
affine(2, 3) = origin.z();
return affine;
}
Eigen::Matrix4f eigenFromTf(const tf::Transform& tf)
{
Eigen::Matrix4f out_mat;
double mv[12];
tf.getBasis().getOpenGLSubMatrix(mv);
tf::Vector3 origin = tf.getOrigin();
out_mat(0, 0) = mv[0]; out_mat(0, 1) = mv[4]; out_mat(0, 2) = mv[8];
out_mat(1, 0) = mv[1]; out_mat(1, 1) = mv[5]; out_mat(1, 2) = mv[9];
out_mat(2, 0) = mv[2]; out_mat(2, 1) = mv[6]; out_mat(2, 2) = mv[10];
out_mat(3, 0) = out_mat(3, 1) = out_mat(3, 2) = 0; out_mat(3, 3) = 1;
out_mat(0, 3) = origin.x();
out_mat(1, 3) = origin.y();
out_mat(2, 3) = origin.z();
return out_mat;
}
/**
* Send vision estimate transform to FCU position controller
*/
void send_vision_transform(const tf::Transform &transform, const ros::Time &stamp) {
// origin and RPY in ENU frame
tf::Vector3 position = transform.getOrigin();
double roll, pitch, yaw;
tf::Matrix3x3 orientation(transform.getBasis());
orientation.getRPY(roll, pitch, yaw);
/* Issue #60.
* Note: this now affects pose callbacks too, but i think its not big deal.
*/
if (last_transform_stamp == stamp) {
ROS_DEBUG_THROTTLE_NAMED(10, "position", "Vision: Same transform as last one, dropped.");
return;
}
last_transform_stamp = stamp;
// TODO: check conversion. Issue #49.
vision_position_estimate(stamp.toNSec() / 1000,
position.y(), position.x(), -position.z(),
roll, -pitch, -yaw); // ??? please check!
}
void PathPlanningWidget::setAddPointUIStartPos(const std::string robot_model_frame,const tf::Transform end_effector)
{
/*! Setting the default values for the Add New Way-Point from the RQT.
The information is taken to correspond to the pose of the loaded Robot end-effector.
*/
tf::Vector3 p = end_effector.getOrigin();
tfScalar rx,ry,rz;
end_effector.getBasis().getRPY(rx,ry,rz,1);
rx = RAD2DEG(rx);
ry = RAD2DEG(ry);
rz = RAD2DEG(rz);
//set up the starting values for the lineEdit of the positions
ui_.LineEditX->setText(QString::number(p.x()));
ui_.LineEditY->setText(QString::number(p.y()));
ui_.LineEditZ->setText(QString::number(p.z()));
//set up the starting values for the lineEdit of the orientations, in Euler angles
ui_.LineEditRx->setText(QString::number(rx));
ui_.LineEditRy->setText(QString::number(ry));
ui_.LineEditRz->setText(QString::number(rz));
}
void TrajectoryPublisher::publishOdometryMsg(tf::Transform transform, ros::Time stamp, std::string frameId,
std::string childFrameId, const Eigen::Matrix<double, 6, 6>& poseCov)
{
// Jan for darpa
std::string world_frame = "odom";
// Jan
// std::string world_frame = "world";
// std::string world_frame = "world_corrected";
double roll, pitch, yaw;
Eigen::Affine3f eigenTrans;
// for ( unsigned int i = 0; i < 3; i++ )
// for ( unsigned int j = 0; j < 3; j++ )
// eigenTrans (transform.getBasis())(i);
//
// z
tf::Vector3 origin = transform.getOrigin();
tf::Matrix3x3 basis = transform.getBasis();
basis.getRPY(roll, pitch, yaw);
tf::Quaternion q;
q.setRPY(roll, pitch, yaw);
nav_msgs::Odometry::Ptr odometryMsg(new nav_msgs::Odometry);
odometryMsg->header.stamp = stamp;
odometryMsg->header.frame_id = frameId;
odometryMsg->pose.pose.position.x = origin.getX();
odometryMsg->pose.pose.position.y = origin.getY();
odometryMsg->pose.pose.position.z = origin.getZ();
odometryMsg->pose.pose.orientation.x = q.x();
odometryMsg->pose.pose.orientation.y = q.y();
odometryMsg->pose.pose.orientation.z = q.z();
odometryMsg->pose.pose.orientation.w = q.w();
// ROS_INFO_STREAM("publishing childFrameid: " << childFrameId << " framid: " << frameId );
m_odometryPublisherMap[childFrameId].publish(odometryMsg);
geometry_msgs::PoseStamped poseChildFrame, poseWorldFrame;
poseChildFrame.header.stamp = stamp;
poseChildFrame.header.frame_id = frameId;
poseChildFrame.pose = odometryMsg->pose.pose;
if (m_transformListener.waitForTransform(world_frame, frameId, stamp, ros::Duration(0.2f)))
{
try
{
m_transformListener.transformPose(world_frame, poseChildFrame, poseWorldFrame);
(*m_fileStreamMap[childFrameId]) << stamp << ", " << poseWorldFrame.pose.position.x << ", "
<< poseWorldFrame.pose.position.y << ", " << poseWorldFrame.pose.position.z
<< ", " << poseWorldFrame.pose.orientation.x << ", "
<< poseWorldFrame.pose.orientation.y << ", " << poseWorldFrame.pose.orientation.z
<< ", " << poseWorldFrame.pose.orientation.w << std::endl;
odometryMsg->pose.pose = poseWorldFrame.pose;
m_transformsMap[childFrameId].push_back(*odometryMsg);
m_covariancesMap[childFrameId].push_back(poseCov);
// offset test HACK
if (childFrameId == "/laser_odom" || childFrameId == "/vis_odom2")
{
// if ( m_transformsMap["/vis_odom2"].size() > 0 ) {
// int n = m_transformsMap["/vis_odom2"].size() -1 ;
// geometry_msgs::Point p;
// p.x = m_transformsMap["/vis_odom2"].at(n).pose.pose.position.x ;
// p.y = m_transformsMap["/vis_odom2"].at(n).pose.pose.position.y ;
// p.z = m_transformsMap["/vis_odom2"].at(n).pose.pose.position.z ;
//
//
// m_offsetHackStream << p.x << ", " << p.y << ", " << p.z << std::endl;
// }
/*
* find closest mocap pose since we are not sure about timing.
*/
// geometry_msgs::PointStamped pLaserLaserFrame, pLaserMocapFrame;
// pLaserLaserFrame.header.stamp = stamp;
// pLaserLaserFrame.header.frame_id = frameId;
// pLaserLaserFrame.point.x = origin.getX();
// pLaserLaserFrame.point.y = origin.getY();
// pLaserLaserFrame.point.z = origin.getZ();
//
// pLaserMocapFrame.header.stamp = stamp;
// pLaserMocapFrame.header.frame_id = "/world";
//
// pcl::PointXYZ pLaserMocap;
//
// if ( m_transformListener.waitForTransform( "/world", frameId, stamp, ros::Duration( 0.2f ) ) ) {
//
// try {
// m_transformListener.transformPoint( "/world", pLaserLaserFrame, pLaserMocapFrame );
// pcl::PointXYZ pLaser;
// pLaser.x = pLaserMocapFrame.point.x ;
// pLaser.y = pLaserMocapFrame.point.y ;
// pLaser.z = pLaserMocapFrame.point.z ;
//
//
float minDist = FLT_MAX;
// for ( unsigned int i = 0; i < m_transformsMap["/mocap"].size(); i++ ) {
// if ( m_transformsMap["/mocap"].size() > 0 ) {
// int i = m_transformsMap["/mocap"].size() -1 ;
//
// pcl::PointXYZ p;
// p.x = m_transformsMap["/mocap"].at(i).pose.pose.position.x ;
// p.y = m_transformsMap["/mocap"].at(i).pose.pose.position.y ;
// p.z = m_transformsMap["/mocap"].at(i).pose.pose.position.z ;
//
// float dist = euclDist ( pLaser, p ) ; //sqrt ( ( pLaser.x - p.x ) * ( pLaser.x - p.x ) + (
//pLaser.y - p.y ) * ( pLaser.y - p.y ) + ( pLaser.z - p.z ) * ( pLaser.z - p.z ) );
//
// if ( dist < minDist)
// minDist = dist;
// }
Eigen::Matrix4f eigenTransform = getMatrixForTf(m_transformListener, world_frame, "/mocap", stamp);
float x, y, z, roll, pitch, yaw;
Eigen::Affine3f trans(eigenTransform);
pcl::getTranslationAndEulerAngles(trans, x, y, z, roll, pitch, yaw);
minDist = sqrt((poseWorldFrame.pose.position.x - x) * (poseWorldFrame.pose.position.x - x) +
(poseWorldFrame.pose.position.y - y) * (poseWorldFrame.pose.position.y - y) +
(poseWorldFrame.pose.position.z - z) * (poseWorldFrame.pose.position.z - z));
if (childFrameId == "/laser_odom")
{
m_laserDistPlot << stamp << ", " << minDist << std::endl;
}
if (childFrameId == "/vis_odom2")
{
m_visOdomDistPlot << stamp << ", " << minDist << std::endl;
}
// }
// catch (tf::TransformException ex)
// {
// ROS_WARN("TrajectoryPublisher: transofrm error: %s", ex.what());
// }
// } else {
// ROS_WARN("TrajectoryPublisher: could not wait for transform for childframe %s",
//childFrameId.c_str());
//
// }
//
}
}
catch (tf::TransformException ex)
{
ROS_WARN("TrajectoryPublisher: transofrm error: %s", ex.what());
}
}
else
{
ROS_WARN("TrajectoryPublisher: could not wait for transform for childframe %s", childFrameId.c_str());
}
visualization_msgs::Marker points, line_strip;
points.header.frame_id = line_strip.header.frame_id = world_frame;
points.header.stamp = line_strip.header.stamp = stamp;
points.ns = line_strip.ns = "points_and_lines";
points.action = line_strip.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w = line_strip.pose.orientation.w = 1.0;
points.id = 0;
line_strip.id = 1;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
// POINTS markers use x and y scale for width/height respectively
points.scale.x = 0.03;
points.scale.y = 0.03;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
line_strip.scale.x = 0.025;
points.color.r = 0.0f;
points.color.g = 1.0f;
points.color.a = 1.0f;
if (childFrameId == "/laser_odom")
{
line_strip.color.r = 1.0f;
line_strip.color.g = 0.0f;
line_strip.color.b = 0.0f;
line_strip.color.a = 1.0f;
}
else if (childFrameId == "/mocap")
{
line_strip.color.r = 1.0f;
line_strip.color.g = 0.0f;
line_strip.color.b = 1.0f;
line_strip.color.a = 1.0f;
points.color.r = 1.0f;
points.color.g = 0.0f;
points.color.b = 0.5f;
points.color.a = 1.0f;
}
else
{
line_strip.color.r = 0.0f;
line_strip.color.g = 0.0f;
line_strip.color.b = 1.0f;
line_strip.color.a = 1.0f;
}
// if (childFrameId == "/laser_odom" ) {
// ROS_INFO_STREAM("publishing laser odom: " << m_transformsMap[childFrameId].size() );
// }
// int markerId = 0;
// Create the vertices for the points and lines
for (uint32_t i = 0; i < m_transformsMap[childFrameId].size(); ++i)
{
geometry_msgs::Point p;
p.x = m_transformsMap[childFrameId].at(i).pose.pose.position.x;
p.y = m_transformsMap[childFrameId].at(i).pose.pose.position.y;
p.z = m_transformsMap[childFrameId].at(i).pose.pose.position.z;
points.points.push_back(p);
line_strip.points.push_back(p);
// Eigen::Matrix<double, 3, 3> cov = m_covariancesMap[childFrameId].at(i).block(0,0,3,3);
// if (cov.trace() == 0)
// continue;
// Eigen::EigenSolver<Eigen::Matrix3d> solver(cov);
// Eigen::Matrix<double, 3, 3> eigenvectors = solver.eigenvectors().real();
// // double eigenvalues[3];
// Eigen::Matrix<double, 3, 1> eigenvalues = solver.eigenvalues().real();
// // for(int j = 0; j < 3; ++j) {
// // Eigen::Matrix<double, 3, 1> mult = cov * eigenvectors.col(j);
// // eigenvalues[j] = mult(0,0) / eigenvectors.col(j)(0);
// // }
// if (eigenvectors.determinant() < 0) {
// eigenvectors.col(0)(0) = -eigenvectors.col(0)(0);
// eigenvectors.col(0)(1) = -eigenvectors.col(0)(1);
// eigenvectors.col(0)(2) = -eigenvectors.col(0)(2);
// }
// Eigen::Quaternion<double> q = Eigen::Quaternion<double>(eigenvectors);
//
// visualization_msgs::Marker marker;
// marker.header.frame_id = "/world";
// marker.header.stamp = stamp;
// marker.ns = "covariance";
// marker.id = markerId++;
// marker.type = marker.SPHERE;
// marker.action = marker.ADD;
// marker.pose.position.x = p.x;
// marker.pose.position.y = p.y;
// marker.pose.position.z = p.z;
// marker.pose.orientation.w = q.w();
// marker.pose.orientation.x = q.x();
// marker.pose.orientation.y = q.y();
// marker.pose.orientation.z = q.z();
// marker.scale.x = sqrt(eigenvalues[0]) * 3;
// marker.scale.y = sqrt(eigenvalues[1]) * 3;
// marker.scale.z = sqrt(eigenvalues[2]) * 3;
// marker.color.r = std::min( 1.0, sqrt(eigenvalues[0]) * 3 );
// marker.color.g = std::min( 1.0, sqrt(eigenvalues[1]) * 3 );
// marker.color.b = std::min( 1.0, sqrt(eigenvalues[2]) * 3 );
// marker.color.a = 1.0;
//
//// double dot = surfel.normal_.dot(Eigen::Vector3d(0.,0.,1.));
//// if( surfel.normal_.norm() > 1e-10 )
//// dot /= surfel.normal_.norm();
//// double angle = acos(fabs(dot));
//// double angle_normalized = 2. * angle / M_PI;
//// marker.color.r = ColorMapJet::red(angle_normalized); //fabs(surfel.normal_(0));
//// marker.color.g = ColorMapJet::green(angle_normalized);
//// marker.color.b = ColorMapJet::blue(angle_normalized);
//
// m_markerPublisherMap[childFrameId].publish( marker );
}
m_markerPublisherMap[childFrameId].publish(points);
m_markerPublisherMap[childFrameId].publish(line_strip);
}
void getTfDifference(const tf::Transform& motion, double& dist, double& angle)
{
dist = motion.getOrigin().length();
double trace = motion.getBasis()[0][0] + motion.getBasis()[1][1] + motion.getBasis()[2][2];
angle = acos(std::min(1.0, std::max(-1.0, (trace - 1.0)/2.0)));
}
void PathPlanningWidget::insertRow(const tf::Transform& point_pos,const int count)
{
/*! Whenever we have a new Way-Point insereted either from the RViz or the RQT Widget the the TreeView needs to update the information and insert new row that corresponds to the new insered point.
This function takes care of parsing the data recieved from the RViz or the RQT widget and creating new row with the appropriate data format and Children. One for the position giving us the current position of the Way-Point in all the axis.
One child for the orientation giving us the Euler Angles of each axis.
*/
ROS_INFO("inserting new row in the TreeView");
QAbstractItemModel *model = ui_.treeView->model();
//convert the quartenion to roll pitch yaw angle
tf::Vector3 p = point_pos.getOrigin();
tfScalar rx,ry,rz;
point_pos.getBasis().getRPY(rx,ry,rz,1);
if(count == 0)
{
model->insertRow(count,model->index(count, 0));
model->setData(model->index(0,0,QModelIndex()),QVariant("add_point_button"),Qt::EditRole);
pointRange();
}
else
{
//ROS_INFO_STREAM("Quartenion at add_row: "<<orientation.x()<<"; "<<orientation.y()<<"; "<<orientation.z()<<"; "<<orientation.w()<<";");
if(!model->insertRow(count,model->index(count, 0))) //&& count==0
{
return;
}
//set the strings of each axis of the position
QString pos_x = QString::number(p.x());
QString pos_y = QString::number(p.y());
QString pos_z = QString::number(p.z());
//repeat that with the orientation
QString orient_x = QString::number(RAD2DEG(rx));
QString orient_y = QString::number(RAD2DEG(ry));
QString orient_z = QString::number(RAD2DEG(rz));
model->setData(model->index(count,0),QVariant(count),Qt::EditRole);
//add a child to the last inserted item. First add children in the treeview that
//are just telling the user that if he expands them he can see details about the position and orientation of each point
QModelIndex ind = model->index(count, 0);
model->insertRows(0, 2, ind);
QModelIndex chldind_pos = model->index(0, 0, ind);
QModelIndex chldind_orient = model->index(1, 0, ind);
model->setData(chldind_pos, QVariant("Position"), Qt::EditRole);
model->setData(chldind_orient, QVariant("Orientation"), Qt::EditRole);
//*****************************Set the children for the position**********************************************************
//now add information about each child separately. For the position we have coordinates for X,Y,Z axis.
//therefore we add 3 rows of information
model->insertRows(0, 3, chldind_pos);
//next we set up the data for each of these columns. First the names
model->setData(model->index(0, 0, chldind_pos), QVariant("X:"), Qt::EditRole);
model->setData(model->index(1, 0, chldind_pos), QVariant("Y:"), Qt::EditRole);
model->setData(model->index(2, 0, chldind_pos), QVariant("Z:"), Qt::EditRole);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 1, chldind_pos), QVariant(pos_x), Qt::EditRole);
model->setData(model->index(1, 1, chldind_pos), QVariant(pos_y), Qt::EditRole);
model->setData(model->index(2, 1, chldind_pos), QVariant(pos_z), Qt::EditRole);
//***************************************************************************************************************************
//*****************************Set the children for the orientation**********************************************************
//now we repeat everything again,similar as the position for adding the children for the orientation
model->insertRows(0, 3, chldind_orient);
//next we set up the data for each of these columns. First the names
model->setData(model->index(0, 0, chldind_orient), QVariant("Rx:"), Qt::EditRole);
model->setData(model->index(1, 0, chldind_orient), QVariant("Ry:"), Qt::EditRole);
model->setData(model->index(2, 0, chldind_orient), QVariant("Rz:"), Qt::EditRole);
//second we add the current position information, for each position axis separately
model->setData(model->index(0, 2, chldind_orient), QVariant(orient_x), Qt::EditRole);
model->setData(model->index(1, 2, chldind_orient), QVariant(orient_y), Qt::EditRole);
model->setData(model->index(2, 2, chldind_orient), QVariant(orient_z), Qt::EditRole);
//****************************************************************************************************************************
pointRange();
}
}