diff --git a/tf/include/tf/tf.h b/tf/include/tf/tf.h index 83df2420..f706ca1f 100644 --- a/tf/include/tf/tf.h +++ b/tf/include/tf/tf.h @@ -57,7 +57,7 @@ std::string resolve(const std::string& prefix, const std::string& frame_name); std::string strip_leading_slash(const std::string& frame_name); /** \deprecated This has been renamed to tf::resolve */ -__attribute__((deprecated)) static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} ; +__attribute__((deprecated)) static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR}; @@ -409,7 +409,7 @@ inline void assertQuaternionValid(const tf::Quaternion & q) ss << "Quaternion malformed, magnitude: " << q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() << " should be 1.0" < bool operator==(const Stamped &a, const Stamped &b) { return a.frame_id_ == b.frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); -}; +} /** \brief The Stamped Transform datatype used by tf */ @@ -98,7 +98,7 @@ class StampedTransform : public tf::Transform /** \brief Comparison operator for StampedTransform */ static inline bool operator==(const StampedTransform &a, const StampedTransform &b) { return a.frame_id_ == b.frame_id_ && a.child_frame_id_ == b.child_frame_id_ && a.stamp_ == b.stamp_ && static_cast(a) == static_cast(b); -}; +} /** \brief convert Quaternion msg to Quaternion */ @@ -110,7 +110,7 @@ static inline void quaternionMsgToTF(const geometry_msgs::Quaternion& msg, Quate ROS_WARN("MSG to TF: Quaternion Not Properly Normalized"); bt.normalize(); } -}; +} /** \brief convert Quaternion to Quaternion msg*/ static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quaternion& msg) { @@ -125,7 +125,7 @@ static inline void quaternionTFToMsg(const Quaternion& bt, geometry_msgs::Quater { msg.x = bt.x(); msg.y = bt.y(); msg.z = bt.z(); msg.w = bt.w(); } -}; +} /** \brief Helper function for getting yaw from a Quaternion */ static inline double getYaw(const Quaternion& bt_q){ @@ -195,68 +195,68 @@ static inline tf::Quaternion createIdentityQuaternion() Quaternion q; q.setRPY(0,0,0); return q; -}; +} /** \brief convert QuaternionStamped msg to Stamped */ static inline void quaternionStampedMsgToTF(const geometry_msgs::QuaternionStamped & msg, Stamped& bt) -{quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; +{quaternionMsgToTF(msg.quaternion, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to QuaternionStamped msg*/ static inline void quaternionStampedTFToMsg(const Stamped& bt, geometry_msgs::QuaternionStamped & msg) -{quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; +{quaternionTFToMsg(bt, msg.quaternion); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Vector3 msg to Vector3 */ -static inline void vector3MsgToTF(const geometry_msgs::Vector3& msg_v, Vector3& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);}; +static inline void vector3MsgToTF(const geometry_msgs::Vector3& msg_v, Vector3& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);} /** \brief convert Vector3 to Vector3 msg*/ -static inline void vector3TFToMsg(const Vector3& bt_v, geometry_msgs::Vector3& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();}; +static inline void vector3TFToMsg(const Vector3& bt_v, geometry_msgs::Vector3& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();} /** \brief convert Vector3Stamped msg to Stamped */ static inline void vector3StampedMsgToTF(const geometry_msgs::Vector3Stamped & msg, Stamped& bt) -{vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; +{vector3MsgToTF(msg.vector, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to Vector3Stamped msg*/ static inline void vector3StampedTFToMsg(const Stamped& bt, geometry_msgs::Vector3Stamped & msg) -{vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; +{vector3TFToMsg(bt, msg.vector); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Point msg to Point */ -static inline void pointMsgToTF(const geometry_msgs::Point& msg_v, Point& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);}; +static inline void pointMsgToTF(const geometry_msgs::Point& msg_v, Point& bt_v) {bt_v = Vector3(msg_v.x, msg_v.y, msg_v.z);} /** \brief convert Point to Point msg*/ -static inline void pointTFToMsg(const Point& bt_v, geometry_msgs::Point& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();}; +static inline void pointTFToMsg(const Point& bt_v, geometry_msgs::Point& msg_v) {msg_v.x = bt_v.x(); msg_v.y = bt_v.y(); msg_v.z = bt_v.z();} /** \brief convert PointStamped msg to Stamped */ static inline void pointStampedMsgToTF(const geometry_msgs::PointStamped & msg, Stamped& bt) -{pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; +{pointMsgToTF(msg.point, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to PointStamped msg*/ static inline void pointStampedTFToMsg(const Stamped& bt, geometry_msgs::PointStamped & msg) -{pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; +{pointTFToMsg(bt, msg.point); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;} /** \brief convert Transform msg to Transform */ static inline void transformMsgToTF(const geometry_msgs::Transform& msg, Transform& bt) -{bt = Transform(Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), Vector3(msg.translation.x, msg.translation.y, msg.translation.z));}; +{bt = Transform(Quaternion(msg.rotation.x, msg.rotation.y, msg.rotation.z, msg.rotation.w), Vector3(msg.translation.x, msg.translation.y, msg.translation.z));} /** \brief convert Transform to Transform msg*/ static inline void transformTFToMsg(const Transform& bt, geometry_msgs::Transform& msg) -{vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);}; +{vector3TFToMsg(bt.getOrigin(), msg.translation); quaternionTFToMsg(bt.getRotation(), msg.rotation);} /** \brief convert TransformStamped msg to tf::StampedTransform */ static inline void transformStampedMsgToTF(const geometry_msgs::TransformStamped & msg, StampedTransform& bt) -{transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;}; +{transformMsgToTF(msg.transform, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id; bt.child_frame_id_ = msg.child_frame_id;} /** \brief convert tf::StampedTransform to TransformStamped msg*/ static inline void transformStampedTFToMsg(const StampedTransform& bt, geometry_msgs::TransformStamped & msg) -{transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;}; +{transformTFToMsg(bt, msg.transform); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_; msg.child_frame_id = bt.child_frame_id_;} /** \brief convert Pose msg to Pose */ static inline void poseMsgToTF(const geometry_msgs::Pose& msg, Pose& bt) -{bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, msg.position.y, msg.position.z));}; +{bt = Transform(Quaternion(msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w), Vector3(msg.position.x, msg.position.y, msg.position.z));} /** \brief convert Pose to Pose msg*/ static inline void poseTFToMsg(const Pose& bt, geometry_msgs::Pose& msg) -{pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);}; +{pointTFToMsg(bt.getOrigin(), msg.position); quaternionTFToMsg(bt.getRotation(), msg.orientation);} /** \brief convert PoseStamped msg to Stamped */ static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped& bt) -{poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;}; +{poseMsgToTF(msg.pose, bt); bt.stamp_ = msg.header.stamp; bt.frame_id_ = msg.header.frame_id;} /** \brief convert Stamped to PoseStamped msg*/ static inline void poseStampedTFToMsg(const Stamped& bt, geometry_msgs::PoseStamped & msg) -{poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}; +{poseTFToMsg(bt, msg.pose); msg.header.stamp = bt.stamp_; msg.header.frame_id = bt.frame_id_;}