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/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::Pose2D<double> &pose_update, ecl::linear_algebra::Vector3d &pose_update_rates); 00045 void resetOdometry() { pose.setIdentity(); } 00046 const ros::Duration& timeout() const { return cmd_vel_timeout; } 00047 void resetTimeout() { last_cmd_time = ros::Time::now(); } 00048 00049 private: 00050 geometry_msgs::TransformStamped odom_trans; 00051 ecl::Pose2D<double> pose; 00052 std::string odom_frame; 00053 std::string base_frame; 00054 ros::Duration cmd_vel_timeout; 00055 ros::Time last_cmd_time; 00056 bool publish_tf; 00057 tf::TransformBroadcaster odom_broadcaster; 00058 ros::Publisher odom_publisher; 00059 00060 void publishTransform(const geometry_msgs::Quaternion &odom_quat); 00061 void publishOdometry(const geometry_msgs::Quaternion &odom_quat, const ecl::linear_algebra::Vector3d &pose_update_rates); 00062 }; 00063 00064 } // namespace kobuki 00065 00066 #endif /* KOBUKI_NODE_ODOMETRY_HPP_ */