00001 00009 /***************************************************************************** 00010 ** Ifdefs 00011 *****************************************************************************/ 00012 00013 #ifndef KOBUKI_NODE_ODOMETRY_HPP_ 00014 #define KOBUKI_NODE_ODOMETRY_HPP_ 00015 00016 /***************************************************************************** 00017 ** Includes 00018 *****************************************************************************/ 00019 00020 #include <string> 00021 #include <geometry_msgs/Twist.h> 00022 #include <nav_msgs/Odometry.h> 00023 #include <tf/transform_broadcaster.h> 00024 #include <ecl/geometry/legacy_pose2d.hpp> 00025 00026 /***************************************************************************** 00027 ** Namespaces 00028 *****************************************************************************/ 00029 00030 namespace kobuki { 00031 00032 /***************************************************************************** 00033 ** Interfaces 00034 *****************************************************************************/ 00035 00039 class Odometry { 00040 public: 00041 Odometry(); 00042 void init(ros::NodeHandle& nh, const std::string& name); 00043 bool commandTimeout() const; 00044 void update(const ecl::LegacyPose2D<double> &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates, 00045 double imu_heading, double imu_angular_velocity); 00046 void resetOdometry() { pose.setIdentity(); } 00047 const ros::Duration& timeout() const { return cmd_vel_timeout; } 00048 void resetTimeout() { last_cmd_time = ros::Time::now(); } 00049 00050 private: 00051 geometry_msgs::TransformStamped odom_trans; 00052 ecl::LegacyPose2D<double> pose; 00053 std::string odom_frame; 00054 std::string base_frame; 00055 ros::Duration cmd_vel_timeout; 00056 ros::Time last_cmd_time; 00057 bool publish_tf; 00058 bool use_imu_heading; 00059 tf::TransformBroadcaster odom_broadcaster; 00060 ros::Publisher odom_publisher; 00061 00062 void publishTransform(const geometry_msgs::Quaternion &odom_quat); 00063 void publishOdometry(const geometry_msgs::Quaternion &odom_quat, const ecl::linear_algebra::Vector3d &pose_update_rates); 00064 }; 00065 00066 } // namespace kobuki 00067 00068 #endif /* KOBUKI_NODE_ODOMETRY_HPP_ */