motion_controller_base.h

Go to the documentation of this file.

/******************************************************************************
   STDR Simulator - Simple Two DImensional Robot Simulator
   Copyright (C) 2013 STDR Simulator
   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 3 of the License, or
   (at your option) any later version.
   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software Foundation,
   Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301  USA
   
   Authors : 
   * Manos Tsardoulias, etsardou@gmail.com
   * Aris Thallas, aris.thallas@gmail.com
   * Chris Zalidis, zalidis@gmail.com 
******************************************************************************/

#ifndef MOTION_CONTROLLER_BASE_H
#define MOTION_CONTROLLER_BASE_H

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Pose2D.h>
#include <stdr_msgs/KinematicMsg.h>

#include <ctime>

namespace stdr_robot {
    

  class MotionController {
    
    public:
      
      virtual void velocityCallback(const geometry_msgs::Twist& msg)
      {
        _currentTwist = msg;
        sampleVelocities();
      }

      virtual void sampleVelocities(void)
      {
        float ux = _currentTwist.linear.x;
        float uy = _currentTwist.linear.y;
        float w = _currentTwist.angular.z;

        float sample_ux = 
          _motion_parameters.a_ux_ux * ux * ux +
          _motion_parameters.a_ux_uy * uy * uy +
          _motion_parameters.a_ux_w  * w  * w;
        _currentTwist.linear.x += sampleNormal(sqrt(sample_ux));

        float sample_uy = 
          _motion_parameters.a_uy_ux * ux * ux +
          _motion_parameters.a_uy_uy * uy * uy +
          _motion_parameters.a_uy_w  * w  * w;
        _currentTwist.linear.y += sampleNormal(sqrt(sample_uy));
 
        float sample_w = 
          _motion_parameters.a_w_ux * ux * ux +
          _motion_parameters.a_w_uy * uy * uy +
          _motion_parameters.a_w_w  * w  * w;
        _currentTwist.angular.z += sampleNormal(sqrt(sample_w));

        float sample_g = 
          _motion_parameters.a_g_ux * ux * ux +
          _motion_parameters.a_g_uy * uy * uy +
          _motion_parameters.a_g_w  * w  * w;
        _currentTwist.angular.z += sampleNormal(sqrt(sample_g));
      }

      
      virtual void stop(void)
      {
        _currentTwist.linear.x = 0;
        _currentTwist.linear.y = 0;
        _currentTwist.linear.z = 0;
        _currentTwist.angular.x = 0;
        _currentTwist.angular.y = 0;
        _currentTwist.angular.z = 0;
      }
      
      virtual void calculateMotion(const ros::TimerEvent& event) = 0;
      
      inline geometry_msgs::Pose2D getPose(void)
      {
        return _pose;
      }
      
      inline void setPose(geometry_msgs::Pose2D new_pose)
      {
        _pose.x = new_pose.x;
        _pose.y = new_pose.y;
        _pose.theta = new_pose.theta;
      }
      
      inline geometry_msgs::Twist getVelocity() {
        return _currentTwist;
      }

      virtual ~MotionController(void) 
      {
      }

      float sampleNormal(float sigma)
      {
        float tmp = 0;
        for (unsigned int i = 0 ; i < 12 ; i++)
        {
          float sample = (rand() % 100000) / 50000.0 - 1.0; // From -1.0 -> 1.0
          tmp += sample * sigma;
        }
        return tmp / 2.0;
      }

    
    protected:
      
      MotionController(
        const geometry_msgs::Pose2D& pose, 
        tf::TransformBroadcaster& tf, 
        const std::string& name,
        ros::NodeHandle& n,
        const stdr_msgs::KinematicMsg params
        )
          : _tfBroadcaster(tf), 
            _freq(0.1), 
            _namespace(name),
            _pose(pose),
            _motion_parameters(params)
        { 
          _velocitySubscrider = n.subscribe(
            _namespace + "/cmd_vel",
            1,
            &MotionController::velocityCallback,
            this);  
 
          srand(time(NULL));
        }

    protected:
      
      const std::string& _namespace;
      ros::Subscriber _velocitySubscrider;
      ros::Duration _freq;
      ros::Timer _calcTimer;
      tf::TransformBroadcaster& _tfBroadcaster;
      geometry_msgs::Pose2D _pose;
      geometry_msgs::Twist _currentTwist;
      stdr_msgs::KinematicMsg _motion_parameters;
  };
    
  typedef boost::shared_ptr<MotionController> MotionControllerPtr;
    
}


#endif