32 #include <sensor_msgs/Imu.h> 33 #include <geometry_msgs/PoseStamped.h> 34 #include <nav_msgs/Odometry.h> 60 #ifndef TF_MATRIX3x3_H 71 tfScalar imu_yaw, imu_pitch, imu_roll;
78 quat.
setRPY(imu_roll, imu_pitch, 0.0);
85 tfScalar pose_yaw, pose_pitch, pose_roll;
95 orientation_quaternion_.
setRPY(imu_roll, imu_pitch, pose_yaw);
110 odom_msg_.header.stamp = imu_msg->header.stamp;
123 std::vector<tf::StampedTransform> transforms;
124 transforms.resize(2);
129 robot_pose_transform_.
setRotation(robot_pose_quaternion_);
130 robot_pose_transform_.
setOrigin(robot_pose_position_);
148 double height_difference =
149 (-plane_normal.
getX() * (robot_pose_position_.
getX() - last_position.
getX())
150 -plane_normal.
getY() * (robot_pose_position_.
getY() - last_position.
getY())
151 +plane_normal.
getZ() * last_position.
getZ()) / last_position.
getZ();
161 int main(
int argc,
char **argv) {
177 fused_imu_publisher_ = n.
advertise<sensor_msgs::Imu>(
"/fused_imu",1,
false);
178 odometry_publisher_ = n.
advertise<nav_msgs::Odometry>(
"/state", 1,
false);
static void pointMsgToTF(const geometry_msgs::Point &msg_v, Point &bt_v)
void publish(const boost::shared_ptr< M > &message) const
tf::Point robot_pose_position_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
tf::Transform robot_pose_transform_
tf::StampedTransform transform_
tf::TransformBroadcaster * tfB_
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
geometry_msgs::PoseStampedConstPtr last_pose_msg_
ros::Publisher odometry_publisher_
TFSIMD_FORCE_INLINE const tfScalar & getW() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
ROSCPP_DECL void spin(Spinner &spinner)
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr &pose_msg)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
std::string p_base_stabilized_frame_
int main(int argc, char **argv)
void imuMsgCallback(const sensor_msgs::Imu::ConstPtr &imu_msg)
tf::Quaternion orientation_quaternion_
tf::Quaternion robot_pose_quaternion_
std::string p_base_footprint_frame_
sensor_msgs::Imu fused_imu_msg_
std::string p_base_frame_
TFSIMD_FORCE_INLINE const tfScalar & getX() const
nav_msgs::Odometry odom_msg_
sensor_msgs::ImuConstPtr last_imu_msg_
ros::Publisher fused_imu_publisher_