[Documentation] [TitleIndex] [WordIndex] 

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

tf package

math_3d library

//Notes:
//Sachin wants operator overloading.  

// 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.

//  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
{
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

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

Unresolved issues

2024-12-28 18:41