pub_odometry.h

Go to the documentation of this file.

/************************************************************************
 *      Copyright (C) 2012 Eindhoven University of Technology (TU/e).           *
 *      All rights reserved.                                                                                            *
 ************************************************************************
 *      Redistribution and use in source and binary forms, with or without      *
 *      modification, are permitted provided that the following conditions      *
 *      are met:                                                                                                                        *
 *                                                                                                                                              *
 *              1.      Redistributions of source code must retain the above            *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer.                                                                                                     *
 *                                                                                                                                              *
 *              2.      Redistributions in binary form must reproduce the above         *
 *                      copyright notice, this list of conditions and the following *
 *                      disclaimer in the documentation and/or other materials          *
 *                      provided with the distribution.                                                         *
 *                                                                                                                                              *
 *      THIS SOFTWARE IS PROVIDED BY TU/e "AS IS" AND ANY EXPRESS OR            *
 *      IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED          *
 *      WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE      *
 *      ARE DISCLAIMED. IN NO EVENT SHALL TU/e OR CONTRIBUTORS BE LIABLE        *
 *      FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR            *
 *      CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT       *
 *      OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS;         *
 *      OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF           *
 *      LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT                       *
 *      (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE       *
 *      USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH        *
 *      DAMAGE.                                                                                                                         *
 *                                                                                                                                              *
 *      The views and conclusions contained in the software and                         *
 *      documentation are those of the authors and should not be                        *
 *      interpreted as representing official policies, either expressed or      *
 *      implied, of TU/e.                                                                                                       *
 ************************************************************************/

#include <pr2_controller_interface/controller.h>
#include <pr2_mechanism_model/joint.h>
#include <ros/ros.h>
#include <control_toolbox/pid.h>
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/condition.hpp>
#include <realtime_tools/realtime_publisher.h>
#include <tf/transform_broadcaster.h>
#include <pr2_mechanism_controllers/Odometer.h>
#include <pr2_mechanism_controllers/BaseOdometryState.h>

namespace controller
{


  class BaseOdometry : public pr2_controller_interface::Controller
  {
    private:
    void updateOdometry();

    void computeBaseVelocity();

    void publish();

    void getOdometryMessage(nav_msgs::Odometry &msg);

    void populateCovariance(nav_msgs::Odometry &msg);

    void publishTransform();

    ros::NodeHandle n;

    pr2_mechanism_model::RobotState *robot_; 

    boost::scoped_ptr<realtime_tools::RealtimePublisher <nav_msgs::Odometry> > odometry_publisher_ ;

    boost::scoped_ptr<realtime_tools::RealtimePublisher <tf::tfMessage> > transform_publisher_ ;

    double odometry_publish_rate_;
    bool publish_odometry_, publish_tf_ ;

    geometry_msgs::Point odometry_;
    geometry_msgs::Twist odometry_vel_;

    std::string odometry_frame_;
    std::string base_link_frame_;
    std::string base_footprint_frame_;
    std::string tf_prefix_;


    double sigma_x_,sigma_y_,sigma_theta_,cov_x_y_,cov_x_theta_,cov_y_theta_;

    ros::Time last_time_;
    ros::Time current_time_;

    ros::Time last_odometry_publish_time_, last_transform_publish_time_;

    double expected_publish_time_;
    double update_time;

    pr2_mechanism_model::JointState* wheel_inner_left_front_state;
    pr2_mechanism_model::JointState* wheel_inner_left_rear_state;
    pr2_mechanism_model::JointState* wheel_inner_right_front_state;
    pr2_mechanism_model::JointState* wheel_inner_right_rear_state;
    double current_vel_wheel_inner_left_front, current_vel_wheel_inner_right_front, current_vel_wheel_inner_left_rear, current_vel_wheel_inner_right_rear;

    double current_vel_x, current_vel_y, current_vel_phi;

    public:

    bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n);

    void starting();

    void update();

    void stopping();

  };
}