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