This is a first cut at a new proposed structure for the libTF replacements

## tf package

• minimal to no external dependancies
• realtime safe(at least usably so)

### math_3d library

```//Notes:

// A standalone library for basic geometric operations
namespace math_3d
{

class Vector3
{
public:
//Mutators
Vector(x,y,z);
void normalize(void);
void scale(double scale_factor);

//Accessors
double normP(double power); // Pnorm
double normInf(); // Infinity Norm
double norm(void);  // 2 norm

void toSimpleMatrix(NEWMAT::Matrix& mat); // [x,y,z]'
void toMatrix(NEWMAT::Matrix& mat); // [x,y,z,0]'

void toSkewSymmetric(NEWMAT::Matrix& mat); //3x3

//Data
double x, y, z;
};

class Point3
{
public:
Point(double, double, double);

void toSimpleMatrix(NEWMAT::Matrix& mat); // [x,y,z]'
void toMatrix(NEWMAT::Matrix& mat); //[x,y,z,1]'

double x, y, z;
};

class Quaternion fast but requires understanding

class Orientation nice and hand holding (normalize every time) used quaternion inside
{
public:

Orientation();// identity

setIdentity();
void normalize(void);

void invert(void); // -x, -y,-z, w or -w?

void toAxisAngle(Vector&axis, double& angle);
bool toEulerZYX(yaw, pitch, roll, int solution_num=1);
void toQuaternion xyzw, array

void toMatrix( NEWMAT::Matrix & matOut); //4x4
void toSimpleMatrix( NEWMAT::Matrix & matOut);//3x3

void fromAxisAngle(const Vector &axis, double angle);
void fromMatrix(const NEWMAT::Matrix &mat); //3x3 or 4x4
void fromEulerZYX(yaw, pitch, roll);
void fromQuaternion(xyzw, array

private:
double x, y, z, w;
}

//Possible future data types not needed at the moment
// Plane
//  double distancePointPlane
// Box
//  bool pointInBox
// Sphere
//  double distanceSpherePoint
//  bool pointInSphere

// I know code looks MUCH better with operators instead of these
// ugly functions but I really think this kind of code is less
// bug prone and fewer surprises happen.

// Also, avoid returning anything more than simple
// datatypes. Returning a Point structure will require useless
// copying of data. Most of the code should be inlined (written in
// the .h file)

// All of the functions taking a reference as argument for placing
// the result there should be written such that if the output is
// the same as one of the inputs, the function will still work
// correctly.

// a is both an input and an output.

static inline double distancePointPoint(const Point &a, const Point &b);
static inline double distanceSquaredPointPoint(const Point &a, const Point &b); // square of previous

static inline double dotProduct(const Vector &a, const Vector &b);
static inline void scalarProduct(const Vector &v, double scalar, Vector &dest); // multiply everything by a constant
static inline void crossProduct(const Vector &a, const Vector &b, Vector &dest);

static inline void multiplyQuaternionQuaternion(const Quaternion &q1, const Quaternion &q2, Quaternion &qout);

static inline void addPointVector(const Point &p, const Vector &v, Point &dest);
static inline void subPointVector(const Point &p, const Vector &v, Point &dest);
static inline void addVectorVector(const Vector &p, const Vector &v, Vector &dest);
static inline void subVectorVector(const Vector &p, const Vector &v, Vector &dest);
static inline void subPointPoint(const Point &p, const Point &v, Vector &dest);

//Interpolation methods
static inline void interpolatePointPoint(const Point& p1, const Point& p2, const double& fraction,
Point& pOut);
static inline void interpolateVectorVector(const Vector& v1, const Vector& v2, const double& fraction,
Vectpr& vOut);
static inline void slerpQuaternionQuaternion(const Quaternion& q1, const Quaternion& q2, const double& fraction,
Quaterion& qOut);
static inline void quadradicInterpolate(const Point& p1, const Vector& v1, const Point& p2, const Vector& v2, const double & fraction,
Point& p_out, Vector& v_out);//sachin's implementing

static inline void cubicInterpolate(const Point& p1, const Vector& v1, const Vector& a1,
const Point& p2, const Vector& v2, const Vector& a2, const double & fraction,
Point& p_out, Vector& v_out); //sachin's implementing

class Transform
{
public:
Transform(void)
{
// produces identity transform
};

// reinitializing the transform
void setIdentity(void);
void setIdentityPosition(void);
void setIdentityOrientation(void);

void fromPosition(const Point &trans);// orientation zeroed
void fromOrientation(const Quaternion &quat);// position zeroed
void fromEuler(const Euler & euler);//position zeroed
void fromAxisAngle(const Vector &axis, double angle);// position zeroed

void fromAxisAnglePoint(const Vector &axis, double angle, const Point &point);
void fromMatrix(const NEWMAT::Matrix &mat);

void invert();

// conversion to newmat 4x4 matrix
//  r r r x
//  r r r y
//  r r r z
//  0 0 0 1

void toMatrix(NEWMAT::Matrix &mat);

private:
//Data
Quaternion orientation;
Point      position;
};

static inline void transformPoint(const Transform &tf, const Point &pt, Point &dest);
static inline void transformVector(const Transform &tf, const Vector &pt, Vector &dest);
static inline void transformTransform(const Transform &a, const Transform &b, Transform &dest);

static inline void applySimilarityTransform(const Transform & transform, const Transform & target); //used to change coordinate frames

static inline void interpolateTransformTransform(const Transform& t1, const Transform& p2, const double& fraction, Point& pOut);

}```

### tf library

```namespace tf{

/** \brief A base class for all tf exceptions
* This inherits from ros::exception
* which inherits from std::runtime_exception
*/
class TFException(): public ros::exception{};

/** \brief An exception for when the graph is unconnected */
class ConnectivityException():public TFException{};

/** \brief An exception for when a frame doesn't exist */
class LookupException(): public TFException{};

/** \brief An exception for when extrapolating too far */
class ExtrapolationException(): public TFException{};

namespace caching
{
/** \brief The data type which will be cross compatable with std_msgs
* this will require the associated rosTF package to convert */
template <typename T>
class Stamped(){
public:
T data;
uint64_t stamp;
std::string frame_id;

Stamped(const T& input, const uint64_t& timestamp, const std::string & frame_id);
void stripStamp(T & output);
};

/** \brief A class to keep a sorted linked list in time
* This builds and maintains a list of timestamped
* data.  And provides lookup functions to get
* data out as a function of time. */
template <typename T>  //Only expect to use Transform
class TimeCache()
{
public:
TimeCache(bool interpolating = TRUE, uint64_t max_cache_time = DEFAULT_MAX_STORAGE_TIME,
uint64_t max_extrapolation_time = DEFAULT_MAX_EXTRAPOLATION_TIME);

void clearCache(void);

int64_t getData(const uint64_t & time, Stamped<T> & data_out); //return distance
void insertData(const Stamped<T>& data);

void interpolate(const Stamped<T>& one, const Stampe<T>& two, Stamped<T>& output){  };  //specific implementation for each T

private:
std::list<Stamped<T> > storage_;

};

}

namespace graphing{

/** \brief A class to store and provide arbitrary transforms within the system
* This will keep track of transforms recieved from throughout the system, and
* provide frame_id and time based accessors to the transforms.  Dyhamically
* constructing the graph in time.
*/
class Transformer{
public:
void clear(void);
void setTransform(const Stamped<T>& transform, const std::string& parent_id)

void lookupTransform(const std::string& target_frame, const std::string& source_frame,
const uint64_t& time, Stamped<T>& transform);

void lookupTransform(const std::string& target_frame, const uint64_t& target_time,
const std::string& source_frame, const uint64_t& _source_time,
const std::string& fixed_frame, Stamped<T>& transform);

template<typename T>
void transformStamped(const std::string& target_frame, const Stamped<T>& stamped_in, Stamped<T>& stamped_out);

template<typename T>
void transformStamped(const std::string& target_frame, const uint64_t& _target_time,const std::string& fixed_frame,
const Stamped<T>& stamped_in, Stamped<T>& stamped_out);

//debugging
std::string chainAsString(const std::string &target_frame, const std::string &source_frame);
std::string allFramesAsString();

private:
std::map<std::string, unsigned int> name_map_;
std::map<unsigned int, std::string> reverse_name_map_;
std::vector<TimeCache<Transform>> frame_vector_;
}
}

}```

## tf_messaging package

• external dependancies on ros messaging
• python version
• SWIG of tf library with rospy messaging

### tfmessaging library

```namespace tf::ros{
//??stamped ambiguous?
static inline void Pose3DMsgStampedToTFStamped(const robot_msgs::Pose3D& pose, Stamped<Transform> & transform);
static inline void PositionMsgStampedToTFStamped(const robot_msgs::Position& position, Stamped<Point> & tf_point);
static inline void VectorMsgStampedToTFStamped(const robot_msgs::Vector& msg_vector, Stamped<Vector> & tf_vector);
static inline void OrientationMsgStampedToTFStamped(const robot_msgs::Orientation& quaternion, Stamped<Quaternion> & tf_quaternion);

// vice versa
static inline void TFStampedToPose3DMsgStamped(const Stamped<Transform> & transform, robot_msgs::Pose3D& pose);
static inline void TFStampedToPositionMsgStamped(const Stamped<Point> & point, robot_msgs::Position& position);
static inline void TFStampedToOrientationMsgStamped(const Stamped<Quaternion> & quaternion, robot_msgs::Orientation& orientation);
static inline void TFStampedToVectorMsgStamped(const Stamped<Vector> & tfvector, robot_msgs::Vector& vec);

Change to
fromMsg( .. . )
toMsg(   )

class TFClient : public Transformer { //subscribes to message and automatically stores incoming data
public:
TFClient(ros::node& anode);

//Use Transformer interface for Stamped data types

void transformVector(const robot_msgs::Vector & vin, Stamped<Vector> & vout);
void transformOrientation(const robot_msgs::Orientation & oin, robot_msgs::Orientation & oout);
void transformPoint(const robot_msgs::Point & vin, robot_msgs::Point & vout);
void transformPointCloud(const robot_msgs::PointCloud& pcin, robot_msgs::PointCloud& pcout);
void projectAndTransformLaserScan(const robot_msgs::LaserScan& scan_in, robot_msgs::PointCloud& pcout);
//Duplicate for time transforming (add target_time and fixed_frame)

// To handle backed up messages
/* These will take in a list of inputs, transform them and build an output list.
* In the case that the data is older than the caching time the input will be summarily deleted.
* In the case that the data is in the valid range(data or withing extrapolation distance) it will project it.
* in the case that the data is in the future(beyond extrapolation limit) it will leave it in the input queue.  */
void transformVector(std::list<robot_msgs::Vector> & vin, std::vector<robot_msgs::Vector> & vout);
void transformOrientation(std::list<robot_msgs::Orientation> & oin, std::list<robot_msgs::Orientation> & oout);
void transformPoint(std::list<robot_msgs::Point> & vin, std::list<robot_msgs::Point> & vout);
void transformPointCloud(std::list<robot_msgs::PointCloud>& pcin, std::list<robot_msgs::PointCloud>& pcout);
void projectAndTransformLaserScan(std::list<robot_msgs::LaserScan>& scan_in, std::list<robot_msgs::PointCloud>& pcout);
//Duplicate for time transforming (add target_time and fixed_frame)

private:
TFArrayMsg msg_in_;
void subscription_callback();

}

class TFSender{
public:
TFSender(ros::node& anode);

//change to list format
sendTransform(const Stamped<Transform> & transform, const std::string& parent_id);
sendTransform(const Transform & transform, const uint64_t & time, const std::string& frame_id, const std::string& parent_id);

}
}```

### Frame ID Management

• Publishers will have a parameter like FRAMEID_LASER which can be arbitrarily set.
• The viewer program will be beefed up and allow real time display of frame ids the graph and possibly the trainsforms(at least the frequency at which they are being recieved).
• If we get fancy they will display the caller id?

### Unresolved issues

• Push down no longer easy since it's not being name mangled.

Wiki: tf/Reviews/2008-09-06_new_proposed_form (last edited 2009-08-14 20:51:35 by localhost)