Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

tf -> tf2 #11

Merged
merged 1 commit into from
Jan 8, 2019
Merged

tf -> tf2 #11

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 7 additions & 4 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,10 @@ find_package(catkin REQUIRED COMPONENTS
moveit_ros_planning
pluginlib
roscpp
tf
tf_conversions
tf2
tf2_eigen
tf2_kdl
tf2_geometry_msgs
)

find_package(OpenMP)
Expand Down Expand Up @@ -68,8 +70,9 @@ catkin_package(
moveit_ros_planning
pluginlib
roscpp
tf
tf_conversions
tf2
tf2_kdl
tf2_geometry_msgs
)

add_compile_options(-frecord-gcc-switches)
Expand Down
2 changes: 0 additions & 2 deletions include/bio_ik/bio_ik.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,6 @@
#include <string>
#include <vector>

#include <tf/tf.h>

#include "goal.h"
#include "goal_types.h"

Expand Down
44 changes: 23 additions & 21 deletions include/bio_ik/frame.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,42 +37,45 @@
#include <vector>

#include <Eigen/Dense>
#include <tf_conversions/tf_kdl.h>
#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_kdl/tf2_kdl.h>

namespace bio_ik
{

typedef tf::Quaternion Quaternion;
typedef tf::Vector3 Vector3;
typedef tf2::Quaternion Quaternion;
typedef tf2::Vector3 Vector3;

struct Frame
{
Vector3 pos;
double __padding[4 - (sizeof(Vector3) / sizeof(double))];
Quaternion rot;
inline Frame() {}
inline Frame(const tf::Vector3& pos, const tf::Quaternion& rot)
inline Frame(const tf2::Vector3& pos, const tf2::Quaternion& rot)
: pos(pos)
, rot(rot)
{
}
explicit inline Frame(const KDL::Frame& kdl)
{
pos = tf::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z());
pos = tf2::Vector3(kdl.p.x(), kdl.p.y(), kdl.p.z());
double qx, qy, qz, qw;
kdl.M.GetQuaternion(qx, qy, qz, qw);
rot = tf::Quaternion(qx, qy, qz, qw);
rot = tf2::Quaternion(qx, qy, qz, qw);
}
explicit inline Frame(const geometry_msgs::Pose& msg)
{
tf::quaternionMsgToTF(msg.orientation, rot);
pos = tf::Vector3(msg.position.x, msg.position.y, msg.position.z);
tf2::fromMsg(msg.orientation, rot);
pos = tf2::Vector3(msg.position.x, msg.position.y, msg.position.z);
}
explicit inline Frame(const Eigen::Affine3d& f)
{
pos = tf::Vector3(f.translation().x(), f.translation().y(), f.translation().z());
pos = tf2::Vector3(f.translation().x(), f.translation().y(), f.translation().z());
Eigen::Quaterniond q(f.rotation());
rot = tf::Quaternion(q.x(), q.y(), q.z(), q.w());
rot = tf2::Quaternion(q.x(), q.y(), q.z(), q.w());
}

inline const Vector3& getPosition() const { return pos; }
Expand All @@ -92,25 +95,24 @@ struct Frame

inline void frameToKDL(const Frame& frame, KDL::Frame& kdl_frame)
{
KDL::Rotation kdl_rot;
KDL::Vector kdl_pos;
tf::quaternionTFToKDL(frame.rot, kdl_rot);
tf::vectorTFToKDL(frame.pos, kdl_pos);
kdl_frame = KDL::Frame(kdl_rot, kdl_pos);
kdl_frame.p.x(frame.pos.x());
kdl_frame.p.y(frame.pos.y());
kdl_frame.p.z(frame.pos.z());
kdl_frame.M = KDL::Rotation::Quaternion(frame.rot.x(), frame.rot.y(), frame.rot.z(), frame.rot.w());
}

template <size_t i> const Frame Frame::IdentityFrameTemplate<i>::identity_frame(Vector3(0, 0, 0), Quaternion(0, 0, 0, 1));

static std::ostream& operator<<(std::ostream& os, const Frame& f) { return os << "(" << f.pos.x() << "," << f.pos.y() << "," << f.pos.z() << ";" << f.rot.x() << "," << f.rot.y() << "," << f.rot.z() << "," << f.rot.w() << ")"; }

__attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q, const tf::Vector3& v, tf::Vector3& r)
__attribute__((always_inline)) inline void quat_mul_vec(const tf2::Quaternion& q, const tf2::Vector3& v, tf2::Vector3& r)
{
double v_x = v.x();
double v_y = v.y();
double v_z = v.z();

// if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf::Vector3(0, 0, 0); return; }
// if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf::Vector3(0, 0, 0); return; }
// if(__builtin_expect(v_x == 0 && v_y == 0 && v_z == 0, 0)) { r = tf2::Vector3(0, 0, 0); return; }
// if(v_x == 0 && v_y == 0 && v_z == 0) { r = tf2::Vector3(0, 0, 0); return; }

double q_x = q.x();
double q_y = q.y();
Expand Down Expand Up @@ -146,7 +148,7 @@ __attribute__((always_inline)) inline void quat_mul_vec(const tf::Quaternion& q,
r.setZ(r_z);
}

__attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p, const tf::Quaternion& q, tf::Quaternion& r)
__attribute__((always_inline)) inline void quat_mul_quat(const tf2::Quaternion& p, const tf2::Quaternion& q, tf2::Quaternion& r)
{
double p_x = p.x();
double p_y = p.y();
Expand All @@ -171,7 +173,7 @@ __attribute__((always_inline)) inline void quat_mul_quat(const tf::Quaternion& p

__attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b, Frame& r)
{
tf::Vector3 d;
tf2::Vector3 d;
quat_mul_vec(a.rot, b.pos, d);
r.pos = a.pos + d;
quat_mul_quat(a.rot, b.rot, r.rot);
Expand All @@ -184,7 +186,7 @@ __attribute__((always_inline)) inline void concat(const Frame& a, const Frame& b
concat(tmp, c, r);
}

__attribute__((always_inline)) inline void quat_inv(const tf::Quaternion& q, tf::Quaternion& r)
__attribute__((always_inline)) inline void quat_inv(const tf2::Quaternion& q, tf2::Quaternion& r)
{
r.setX(-q.x());
r.setY(-q.y());
Expand Down
Loading