pub_odometry.cpp

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 "amigo_gazebo/pub_odometry.h"
#include "pluginlib/class_list_macros.h"

PLUGINLIB_REGISTER_CLASS(BaseOdometryPlugin,
                         controller::BaseOdometry,
                         pr2_controller_interface::Controller)

#define WHEELRAD 0.06
#define DIS2CENT 0.28991378
#define HALFSQRT2 0.7071

using namespace controller;



bool BaseOdometry::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) {
    // get parameters

    std::string prefix_param;
    n.searchParam("tf_prefix", prefix_param);
    n.getParam(prefix_param, tf_prefix_);

    n.param("odom/initial_x", odometry_.x, 0.0);
    n.param("odom/initial_y", odometry_.y, 0.0);
    n.param("odom/initial_yaw", odometry_.z, 0.0);
    n.param<std::string> ("odom_frame", odometry_frame_, "odom");
    n.param<std::string> ("base_link_frame", base_link_frame_, "base_link");
    n.param<std::string> ("base_footprint_frame", base_footprint_frame_, "base_link");

    n.param("odom_publish_rate", odometry_publish_rate_, 200.0);
    n.param("publish_tf", publish_tf_, true);

    n.param<double> ("x_stddev", sigma_x_, 0.002);
    n.param<double> ("y_stddev", sigma_y_, 0.002);
    n.param<double> ("rotation_stddev", sigma_theta_, 0.017);
    n.param<double> ("cov_xy", cov_x_y_, 0.0);
    n.param<double> ("cov_xrotation", cov_x_theta_, 0.0);
    n.param<double> ("cov_yrotation", cov_y_theta_, 0.0);

    std::string wheel_inner_left_front, wheel_inner_right_front, wheel_inner_left_rear, wheel_inner_right_rear;

    //wheel_inner_left_front
    if (!n.getParam("wheel_inner_left_front", wheel_inner_left_front)){
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_left_front_state = robot->getJointState(wheel_inner_left_front);
    if (!wheel_inner_left_front_state ){
        ROS_ERROR("BaseOdometry could not find joint named '%s'",
                  wheel_inner_left_front.c_str());
        return false;
    }

    //wheel_inner_right_front
    if (!n.getParam("wheel_inner_right_front", wheel_inner_right_front)){
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_right_front_state = robot->getJointState(wheel_inner_right_front);
    if (!wheel_inner_right_front_state ){
        ROS_ERROR("BaseOdometry could not find joint named '%s'",
                  wheel_inner_right_front.c_str());
        return false;
    }

    //wheel_inner_left_rear
    if (!n.getParam("wheel_inner_left_rear", wheel_inner_left_rear)){
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_left_rear_state = robot->getJointState(wheel_inner_left_rear);
    if (!wheel_inner_left_rear_state ){
        ROS_ERROR("BaseOdometry could not find joint named '%s'",
                  wheel_inner_left_rear.c_str());
        return false;
    }

    //wheel_inner_right_rear
    if (!n.getParam("wheel_inner_right_rear", wheel_inner_right_rear)){
        ROS_ERROR("No joint given in namespace: '%s')",
                  n.getNamespace().c_str());
        return false;
    }

    wheel_inner_right_rear_state = robot->getJointState(wheel_inner_right_rear);
    if (!wheel_inner_right_rear_state ){
        ROS_ERROR("BaseOdometry could not find joint named '%s'",
                  wheel_inner_right_rear.c_str());
        return false;
    }


    // copy robot pointer so we can access time
    robot_ = robot;

    odometry_publisher_.reset(new realtime_tools::RealtimePublisher<nav_msgs::Odometry>(n,"/odom", 1));
    transform_publisher_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>(n,"/tf", 1));
    transform_publisher_->msg_.transforms.resize(1);

    if(odometry_publish_rate_ <= 0.0){
        expected_publish_time_ = 0.0;
        publish_odometry_ = false;
    }else{
        expected_publish_time_ = 1.0/odometry_publish_rate_;
        publish_odometry_ = true;
    }

    return true;
}


void BaseOdometry::starting() {
    current_time_ = robot_->getTime();
    last_time_ = robot_->getTime();
    last_odometry_publish_time_ = robot_->getTime();
    last_transform_publish_time_ = robot_->getTime();
}


void BaseOdometry::update() {
    current_time_ = robot_->getTime();
    ros::Time update_start = ros::Time::now();
    updateOdometry();

    update_time = (ros::Time::now()-update_start).toSec();
    ros::Time publish_start = ros::Time::now();

    if(publish_odometry_)
        publish();
    if(publish_tf_)
        publishTransform();

    last_time_ = current_time_;
}


void BaseOdometry::stopping() {
}


void BaseOdometry::updateOdometry() {
    double dt = (current_time_ - last_time_).toSec();

    computeBaseVelocity();

    double odom_delta_x = odometry_vel_.linear.x * dt;
    double odom_delta_y = odometry_vel_.linear.y * dt;
    double odom_delta_th = odometry_vel_.angular.z * dt;

    odometry_.x += odom_delta_x;
    odometry_.y += odom_delta_y;
    odometry_.z += odom_delta_th;

    ROS_DEBUG("Odometry:: Position: %f, %f, %f",odometry_.x,odometry_.y,odometry_.z);
}


void BaseOdometry::computeBaseVelocity() {
    current_vel_wheel_inner_left_front = wheel_inner_left_front_state->velocity_;
    current_vel_wheel_inner_right_front = wheel_inner_right_front_state->velocity_;
    current_vel_wheel_inner_left_rear = wheel_inner_left_rear_state->velocity_;
    current_vel_wheel_inner_right_rear = wheel_inner_right_rear_state->velocity_;

    //conversion to current x, y and phi velocities

    current_vel_x = 0.25*WHEELRAD*(current_vel_wheel_inner_right_front + current_vel_wheel_inner_left_front - current_vel_wheel_inner_left_rear - current_vel_wheel_inner_right_rear);
    current_vel_y = 0.25*WHEELRAD*(-current_vel_wheel_inner_right_front + current_vel_wheel_inner_left_front + current_vel_wheel_inner_left_rear - current_vel_wheel_inner_right_rear);

    current_vel_phi = -0.25*WHEELRAD/DIS2CENT*(current_vel_wheel_inner_right_front+current_vel_wheel_inner_left_front+current_vel_wheel_inner_left_rear+current_vel_wheel_inner_right_rear);

    double theta = odometry_.z;
    double costh = cos(theta);
    double sinth = sin(theta);

    //transform to global frame
    odometry_vel_.linear.x = (current_vel_x * costh - current_vel_y * sinth);
    odometry_vel_.linear.y = (current_vel_x * sinth + current_vel_y * costh);
    odometry_vel_.angular.z = current_vel_phi;
}


void BaseOdometry::publish() {
    if(fabs((last_odometry_publish_time_ - current_time_).toSec()) < expected_publish_time_){
        return;
    }
    if(odometry_publisher_->trylock()){
        getOdometryMessage(odometry_publisher_->msg_);
        odometry_publisher_->unlockAndPublish();
        last_odometry_publish_time_ = current_time_;
    }
}


void BaseOdometry::getOdometryMessage(nav_msgs::Odometry &msg) {
    msg.header.frame_id = odometry_frame_;
    msg.header.stamp = current_time_;
    msg.pose.pose.position.x = odometry_.x;
    msg.pose.pose.position.y = odometry_.y;
    msg.pose.pose.position.z = 0.0;

    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(odometry_.z);
    msg.pose.pose.orientation = odom_quat;

    msg.twist.twist.linear.x = current_vel_x;
    msg.twist.twist.linear.y = current_vel_y;
    msg.twist.twist.angular.z = current_vel_phi;
    
    populateCovariance(msg);
}


void BaseOdometry::populateCovariance(nav_msgs::Odometry &msg) {
    //nav_msgs::Odometry has a 6x6 covariance matrix that must be filled partly
    //the relations between x, y and theta are set (they are symmetrical) and
    //selfrelations of course (the diagonal)

    if(fabs(odometry_vel_.linear.x) <= 1e-8 && fabs(odometry_vel_.linear.y) <= 1e-8 && fabs(odometry_vel_.angular.z) <= 1e-8){
        msg.pose.covariance[0] = 1e-12;
        msg.pose.covariance[7] = 1e-12;
        msg.pose.covariance[35] = 1e-12;

        msg.pose.covariance[1] = 1e-12;
        msg.pose.covariance[6] = 1e-12;

        msg.pose.covariance[31] = 1e-12;
        msg.pose.covariance[11] = 1e-12;

        msg.pose.covariance[30] = 1e-12;
        msg.pose.covariance[5] =  1e-12;
    }else{
        msg.pose.covariance[0] = pow(sigma_x_,2);
        msg.pose.covariance[7] = pow(sigma_y_,2);
        msg.pose.covariance[35] = pow(sigma_theta_,2);

        msg.pose.covariance[1] = cov_x_y_;
        msg.pose.covariance[6] = cov_x_y_;

        msg.pose.covariance[31] = cov_y_theta_;
        msg.pose.covariance[11] = cov_y_theta_;

        msg.pose.covariance[30] = cov_x_theta_;
        msg.pose.covariance[5] =  cov_x_theta_;
    }

    msg.pose.covariance[14] = DBL_MAX;
    msg.pose.covariance[21] = DBL_MAX;
    msg.pose.covariance[28] = DBL_MAX;

    msg.twist.covariance = msg.pose.covariance;
}


void BaseOdometry::publishTransform() {
    if(fabs((last_transform_publish_time_ - current_time_).toSec()) < expected_publish_time_){
        return;
    }
    if(transform_publisher_->trylock()){

        geometry_msgs::TransformStamped &out = transform_publisher_->msg_.transforms[0];
        out.header.stamp = current_time_;
        out.header.frame_id = tf::resolve(tf_prefix_,odometry_frame_);//swapped with next
        out.child_frame_id = tf::resolve(tf_prefix_,base_footprint_frame_);//swapped with prev
        out.transform.translation.x = odometry_.x;
        out.transform.translation.y = odometry_.y;
        out.transform.translation.z = 0;
        tf::Quaternion quat_trans;
        quat_trans.setRPY(0.0, 0.0, odometry_.z);

        out.transform.rotation.x = quat_trans.x();
        out.transform.rotation.y = quat_trans.y();
        out.transform.rotation.z = quat_trans.z();
        out.transform.rotation.w = quat_trans.w();

        transform_publisher_->unlockAndPublish();
        last_transform_publish_time_ = current_time_;
    }
}