vpHomogeneousMatrix tfToVisp(tf::StampedTransform matrix_tf){
vpHomogeneousMatrix matrix_visp;
matrix_visp[0][0]=matrix_tf.getBasis()[0][0]; matrix_visp[0][1]=matrix_tf.getBasis()[0][1]; matrix_visp[0][2]=matrix_tf.getBasis()[0][2]; matrix_visp[0][3]=matrix_tf.getOrigin().x();
matrix_visp[1][0]=matrix_tf.getBasis()[1][0]; matrix_visp[1][1]=matrix_tf.getBasis()[1][1]; matrix_visp[1][2]=matrix_tf.getBasis()[1][2]; matrix_visp[1][3]=matrix_tf.getOrigin().y();
matrix_visp[2][0]=matrix_tf.getBasis()[2][0]; matrix_visp[2][1]=matrix_tf.getBasis()[2][1]; matrix_visp[2][2]=matrix_tf.getBasis()[2][2]; matrix_visp[2][3]=matrix_tf.getOrigin().z();
return matrix_visp;
}
void InputOutputLinControl::getRelativeTransformation2D(tf::StampedTransform v1, tf::StampedTransform v2, tf::Transform& relative_transform)
{
double roll, pitch, yaw1, yaw2;
v1.getBasis().getRPY(roll,pitch,yaw1);
v2.getBasis().getRPY(roll,pitch,yaw2);
double a11 = cos(yaw1 - yaw2);
double a12 = sin(yaw1 - yaw2);
double a13 = 0;
double a21 = - a12;
double a22 = a11;
double a23 = 0;
double a31 = 0;
double a32 = 0;
double a33 = 1;
double t1 = v2.getOrigin().getX() - v1.getOrigin().getX() * a11 - v1.getOrigin().getY()*a12;
double t2 = v2.getOrigin().getY() - v1.getOrigin().getY() * a11 + v1.getOrigin().getX()*a12;
double t3 = 0;
relative_transform = tf::Transform(btMatrix3x3(a11,a12,a13,a21,a22,a23,a31,a32,a33),btVector3(t1,t2,t3));
}
void Robot::updateFrom(tf::TransformListener *tfListener) {
using namespace Ogre;
static tf::StampedTransform baseTF;
static Vector3 translation = Vector3::ZERO;
static Quaternion orientation = Quaternion::IDENTITY;
static Quaternion qRot = Quaternion(-sqrt(0.5), 0.0f, sqrt(0.5), 0.0f)*Quaternion(-sqrt(0.5), sqrt(0.5), 0.0f, 0.0f);
static Quaternion qYn90 = Quaternion(Degree(90), Vector3::NEGATIVE_UNIT_Y);
static tfScalar yaw,pitch,roll;
static Matrix3 mRot;
try {
tfListener->lookupTransform("map","base_footprint",ros::Time(0), baseTF);
translation.x = baseTF.getOrigin().x();
translation.y = baseTF.getOrigin().y();
translation.z = baseTF.getOrigin().z();
translation = qRot*translation + Vector3(0.0f, 1.0f, 0.0f);
baseTF.getBasis().getEulerYPR(yaw,pitch,roll);
mRot.FromEulerAnglesYXZ(Radian(yaw),Radian(0.0f),Radian(0.0f));
orientation.FromRotationMatrix(mRot);
orientation = qYn90*orientation;
robot->setPosition(translation);
robot->setOrientation(orientation);
} catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
}
void InputOutputLinControl::getTrueRobotPose(double displacement, tf::StampedTransform robot_poseB, tf::StampedTransform& true_robot_pose)
{
true_robot_pose.frame_id_ = robot_poseB.frame_id_;
true_robot_pose.stamp_ = robot_poseB.stamp_;
double roll, pitch, yaw;
robot_poseB.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
v.setX(robot_poseB.getOrigin().getX() - displacement*cos(yaw));
v.setY(robot_poseB.getOrigin().getY() - displacement*sin(yaw));
v.setZ(robot_poseB.getOrigin().getZ());
true_robot_pose.setOrigin(v);
true_robot_pose.setRotation(robot_poseB.getRotation());
}
void InputOutputLinControl::realRobotPoseB(double displacement, tf::StampedTransform real_robot_pose, tf::StampedTransform& real_robot_poseB)
{
real_robot_poseB.frame_id_ = real_robot_pose.frame_id_;
real_robot_poseB.stamp_ = real_robot_pose.stamp_;
real_robot_poseB.child_frame_id_ = real_robot_pose.child_frame_id_;
tf::Vector3 v;
double roll, pitch, yaw;
real_robot_pose.getBasis().getRPY(roll,pitch,yaw);
v.setX(real_robot_pose.getOrigin().getX() + displacement*cos(yaw));
v.setY(real_robot_pose.getOrigin().getY() + displacement*sin(yaw));
v.setZ(real_robot_pose.getOrigin().getZ());
real_robot_poseB.setOrigin(v);
real_robot_poseB.setRotation(real_robot_pose.getRotation());
}
void foutAppendTF(tf::StampedTransform &transform, std::ofstream &fout)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
fout<< originVector.x() << " "
<< originVector.y() << " "
<< originVector.z() << " "
<< pcl::rad2deg(roll) << " "
<< pcl::rad2deg(pitch) << " "
<< pcl::rad2deg(yaw) << std::endl;
}
bool InputOutputLinControl::updateRobotPoseByModel(double timestep, double linear_vel, double angular_vel, tf::StampedTransform& pose)
{
pose.stamp_ = ros::Time::now();
double roll, pitch, yaw;
pose.getBasis().getRPY(roll,pitch,yaw);
tf::Vector3 v;
double theta = yaw + angular_vel * timestep;
//Eulerian integration
v.setX(pose.getOrigin().getX() + linear_vel * timestep * cos(theta));
v.setY(pose.getOrigin().getY() + linear_vel * timestep * sin(theta));
v.setZ(pose.getOrigin().getZ());
pose.setOrigin(v);
pose.setRotation(tf::createQuaternionFromYaw(theta));
ROS_INFO("New orientation [%f]",theta);
return true;
}
void printTF(tf::StampedTransform &transform, std::string str)
{
tf::Matrix3x3 rotMatrix = transform.getBasis();
tf::Vector3 originVector = transform.getOrigin();
tfScalar roll, pitch, yaw;
rotMatrix.getRPY(roll, pitch, yaw);
ROS_INFO("TF %s (x y z r p y): %f %f %f %f %f %f",
str.c_str(),
originVector.x(),
originVector.y(),
originVector.z(),
pcl::rad2deg(roll),
pcl::rad2deg(pitch),
pcl::rad2deg(yaw));
}
bool InputOutputLinControl::getRobotCommands(double displacement, tf::StampedTransform robot_pose, double k1, double k2, geometry_msgs::Pose poseB, geometry_msgs::Twist velB, double& linear_vel, double& angular_vel)
{
double roll, pitch, yaw;
robot_pose.getBasis().getRPY(roll,pitch,yaw);
double a = cos(yaw);
double b = sin(yaw);
double c = -sin(yaw)/displacement;
double d = cos(yaw)/displacement;
double u1 = velB.linear.x + k1 * (poseB.position.x - robot_pose.getOrigin().getX());
double u2 = velB.linear.y + k2 * (poseB.position.y - robot_pose.getOrigin().getY());
//double u1 = k1 * (poseB.position.x - robot_pose.getOrigin().getX());
//double u2 = k2 * (poseB.position.y - robot_pose.getOrigin().getY());
linear_vel = a * u1 + b * u2;
angular_vel = c * u1 + d * u2;
geometry_msgs::PoseStamped poseR;
poseR.header.frame_id = robot_pose.frame_id_;
poseR.header.stamp = robot_pose.stamp_;
poseR.pose.position.x = robot_pose.getOrigin().getX();
poseR.pose.position.y = robot_pose.getOrigin().getY();
poseR.pose.position.z = robot_pose.getOrigin().getZ();
poseR.pose.orientation.x = robot_pose.getRotation().getX();
poseR.pose.orientation.y = robot_pose.getRotation().getY();
poseR.pose.orientation.z = robot_pose.getRotation().getZ();
poseR.pose.orientation.w = robot_pose.getRotation().getW();
ROS_INFO("traiettoria del robot x [%f]",robot_pose.getOrigin().getX());
ROS_INFO("traiettoria del robot y [%f]",robot_pose.getOrigin().getY());
local_path.poses.push_back(poseR);
local_path_.publish(local_path);
return true;
}
tf::Transform OdomTf::get_tf_from_stamped_tf(tf::StampedTransform sTf) {
tf::Transform tf(sTf.getBasis(), sTf.getOrigin()); //construct a transform using elements of sTf
return tf;
}