C++ (Cpp) Pose::getBasis Examples

Example #1

File: amcl_node.cpp Project: Renda110/AGVC

double
AmclNode::getYaw(tf::Pose& t)
{
    double yaw, pitch, roll;
    t.getBasis().getEulerYPR(yaw,pitch,roll);
    return yaw;
}

Example #2

File: tf_kdl.cpp Project: AhmedAnsariIIT/iitmabhiyanros

 void poseTFToKDL(const tf::Pose& 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];
 }

Example #3

File: amcl_node.cpp Project: SteveHuang27/navigation

double
AmclNode::getYaw(tf::Pose& t)
{
  double yaw, pitch, roll;
  btMatrix3x3 mat = t.getBasis();
  mat.getEulerYPR(yaw,pitch,roll);
  return yaw;
}

Example #4

File: step_cost_key.cpp Project: TRECVT/vigir_footstep_planning_core

void StepCostKey::mirrorPoseOnXPlane(tf::Pose &mirror, const tf::Pose &orig) const
{
  mirror.setBasis(orig.getBasis());
  tf::Matrix3x3& basis = mirror.getBasis();
  basis[0][1] = -basis[0][1];
  basis[1][0] = -basis[1][0];
  basis[2][1] = -basis[2][1];

  mirror.setOrigin(orig.getOrigin());
  tf::Vector3& origin = mirror.getOrigin();
  origin[1] = -origin[1];


//  double r, p, y;
//  tf::Matrix3x3(orig.getRotation()).getRPY(r, p, y);
//  mirror.setRotation(tf::createQuaternionFromRPY(-r, p, -y));

//  tf::Vector3 v = orig.getOrigin();
//  v.setY(-v.getY());
//  mirror.setOrigin(v);


//  tfScalar m[16];
//  ROS_INFO("------------------");
//  mirror.getOpenGLMatrix(m);
//  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
//  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);

//  ROS_INFO("-");
//  orig.getOpenGLMatrix(m);

//  ROS_INFO("%f %f %f %f", m[0], m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", m[1], m[5], m[9], m[13]);
//  ROS_INFO("%f %f %f %f", m[2], m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);

//  ROS_INFO("-");

//  ROS_INFO("%f %f %f %f", m[0], -m[4], m[8], m[12]);
//  ROS_INFO("%f %f %f %f", -m[1], m[5], m[9], -m[13]);
//  ROS_INFO("%f %f %f %f", m[2], -m[6], m[10], m[14]);
//  ROS_INFO("%f %f %f %f", m[3], m[7], m[11], m[15]);
}

Example #5

File: amcl.cpp Project: Robotics-UniBG/MarkerBasedNavigation_Orocos

double AMCLocalizer::getYaw(tf::Pose& t) {
    double yaw, pitch, roll;
    t.getBasis().getEulerYPR(yaw,pitch,roll);
    return yaw;
}