odometry.hpp
Go to the documentation of this file.
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_ */


kobuki_node
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Mon Oct 6 2014 01:32:48