32 #include "sensor_msgs/Imu.h" 40 #ifndef TF_MATRIX3x3_H 52 tmp_.
setRPY(roll, pitch, 0.0);
56 transform_.
stamp_ = imu_msg.header.stamp;
61 int main(
int argc,
char **argv) {
TFSIMD_FORCE_INLINE void setX(tfScalar x)
TFSIMD_FORCE_INLINE void setY(tfScalar y)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE void setZ(tfScalar z)
std::string p_base_stabilized_frame_
int main(int argc, char **argv)
ROSCPP_DECL void spin(Spinner &spinner)
tf::StampedTransform transform_
std::string p_base_frame_
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
tf::TransformBroadcaster * tfB_
void imuMsgCallback(const sensor_msgs::Imu &imu_msg)