darwin_simple_nav_alg.cpp

Go to the documentation of this file.

#include "darwin_simple_nav_alg.h"

#include <math.h>

DarwinSimpleNavAlgorithm::DarwinSimpleNavAlgorithm(void)
{
  this->max_linear_vel=0.05;
  this->max_angular_vel=0.5;
}

DarwinSimpleNavAlgorithm::~DarwinSimpleNavAlgorithm(void)
{
}

void DarwinSimpleNavAlgorithm::config_update(Config& new_cfg, uint32_t level)
{
  this->lock();

  // save the current configuration
  this->config_=new_cfg;
  this->max_linear_vel=new_cfg.max_linear_velocity;
  this->max_angular_vel=new_cfg.max_angular_velocity;
  this->max_linear_accel=new_cfg.max_linear_acceleration;
  
  this->unlock();
}

// DarwinSimpleNavAlgorithm Public API
bool DarwinSimpleNavAlgorithm::compute_robot_speed(T2DPose &target_pose,double &vt, double &vr)
{
  double linear_dist=0.0;
  double angle=0.0;

  vr=0;
  vt=0;
  // compute linear spped
  linear_dist=sqrt((target_pose.x)*(target_pose.x)+(target_pose.y)*(target_pose.y));
  angle=target_pose.theta;
  ROS_INFO("Distance to target: %f, angle to target: %f",linear_dist,angle);
  if(linear_dist>0.05 && fabs(angle)>0.05)
  { 
    if(linear_dist>this->max_linear_vel) 
      vt=this->max_linear_vel;
    else
      vt=linear_dist;
    if(fabs(angle)>this->max_angular_vel)
    {
      if(angle>0)
        vr=this->max_angular_vel;
      else
        vr=-this->max_angular_vel;
    }
    else
      vr=angle;
    return false;
  }
  else if(fabs(angle)>0.05)
  {
    if(fabs(angle)>this->max_angular_vel)
    {
      if(angle>0)
        vr=this->max_angular_vel;
      else
        vr=this->max_angular_vel;
    }
    else
      vr=angle;
    vt=0;
    return false;
  }
  else if(linear_dist>0.05)
  {
    if(linear_dist>this->max_linear_vel) 
      vt=this->max_linear_vel;
    else
      vt=linear_dist;
    vr=0;
    return false;
  }
  else
  {
    vt=0;
    vr=0;
    return true;
  }
}