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

//Sachin wants operator overloading.  

// A standalone library for basic geometric operations
namespace math_3d
class Vector3
  void normalize(void);
  void scale(double scale_factor);

  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

  double x, y, z;
class Point3
  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
  Orientation();// identity

  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

  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.

//  example: add_vector_vector(a,b,a) will add b to a, even though
// 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
    // 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);

  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(){
  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()
  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

  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{
  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);
  std::string chainAsString(const std::string &target_frame, const std::string &source_frame);
  std::string allFramesAsString();

  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
  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)
  TFArrayMsg msg_in_;
  void subscription_callback();


class TFSender{
  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)