planners/rangesensor/PL_Range_Sensor.cpp

Go to the documentation of this file.

#include <synchronization/semaphore.h>
#include "PL_Range_Sensor.h"
#include <gl\gl.h>



PL_Range_Sensor::~PL_Range_Sensor()
{
}

void PL_Range_Sensor::SetCollisionDetector (CD_BasicStyle* collisionDetector)
{
  const CD_Range_Sensor * cd_range_sensor  = dynamic_cast< const CD_Range_Sensor * >( collisionDetector ) ;
  if( cd_range_sensor != NULL )
  {
        //set up the ACA planner which will be used for path planning through free space
    PL_HasCollisionDetector::SetCollisionDetector(collisionDetector);
    pl_juan.PL_HasCollisionDetector::SetCollisionDetector(collisionDetector);
    return;
  }
  //PL_Range_Sensor won't work without CD_Range_Sensor.
  assert(false);
}

bool PL_Range_Sensor::Plan ()
{
  int i;
  Semaphore semaphore( guid );

  //TRACE("\nPL_Range_Sensor::Plan called");
  
  std::vector<Configuration> prevconfigs;
  Configuration current = GetStartConfig();
  Configuration offset ; 
  Configuration loopstartconfig;
  
  //we will return the path given by pl_juan
  path.Clear() ;

  SetTimeLimitInSeconds(120);
  StartTimer();

  prevconfigs.push_back( GetStartConfig() );
  Configuration intermediate = GetGoalConfig();

  //needed to display the local octree vcollide mesh
  dynamic_cast< CD_Range_Sensor* >(collisionDetector)->isMultiThreaded = this->isMultiThreaded;
  dynamic_cast< CD_Range_Sensor* >(collisionDetector)->cWindow_ptr = this->cWindow_ptr;
  dynamic_cast< CD_Range_Sensor* >(collisionDetector)->hglrc_var = this->hglrc_var;

  //utility feature: just take a picture if we're already at the goal
  if( GetStartConfig() == GetGoalConfig() )
  {
    //TRACE("\nstartConfig == goalConfig, just taking a picture");
    collisionDetector->SetConfiguration( GetStartConfig() ) ;
    //take a picture which deletes the planner window information for a time, so grab semaphore
    semaphore.Lock();
    dynamic_cast< CD_Range_Sensor * >(collisionDetector)->Take_Picture( GetStartConfig() );
    semaphore.Unlock();
    return false;
  }
    
  while (1)
  {    
    //try to make it from startConfig to goalConfig, using PL_Juan;
    if (pl_juan.Plan() == true)
    {
      //if made it to goal successfully, exit
      PA_Points* pathp;
          //copy path known by pl_juan to our returned path
      pathp = dynamic_cast<PA_Points*>(const_cast<PathBase*>(pl_juan.GetPath()));
      assert(pathp != NULL);
      path = *pathp;
      return true;
    }
    
    //else, move arm to midpoint between current spot and collision, using PL_Linear's method

        //these finite # steps will simulate movement untill we hit occupied/unknown space
    const long int steps = 50 ;
    
    //on 1st run, current == startConfig;
    loopstartconfig = current;
    offset = intermediate - current ; //IMPROVE: joint limits mean that it won't be as simple as this
    for( i = 0; i <= steps; i++ )
    {
      if( collisionDetector->IsInterfering( current ) )
      {
        break;
      }
      
      current += ( offset * (1.0 / steps ) ) ;
    }

    //if we hit something, move to halfway point between hit and loop start configs
        if (i <= steps)
    {
      current = (current + loopstartconfig) / 2;
    }
    
    //we better not be hitting anything at this halfway point.
    //if we are, then grab another random point and try again.
    if (collisionDetector->IsInterfering( current ) == false)
    {      
      collisionDetector->SetConfiguration( current ) ;
      
      //take a picture which deletes the planner window information for a time, so grab semaphore
      semaphore.Lock();
      dynamic_cast< CD_Range_Sensor * >(collisionDetector)->Take_Picture( current );
          semaphore.Unlock();
    } else
    {
      current = loopstartconfig;
      //TRACE("\nHalfway point interferrence detected");
    }
    
    //GenerateRandomConfig
    intermediate = GenerateRandomConfig() ;
    //check the timer 
    //if not timeout, return to start of the loop
    if( HasTimeLimitExpired() == true )
    {
      return false ;
    }
  }

  //control should never get here
  assert (false);
  return false ;
}

void PL_Range_Sensor::SetStartConfig (const Configuration& configuration)
{
  PL_HasCollisionDetector::SetStartConfig( configuration );
  pl_juan.SetStartConfig( configuration ) ;
}

void PL_Range_Sensor::SetGoalConfig (const Configuration& configuration)
{
  PL_HasCollisionDetector::SetGoalConfig( configuration );
  pl_juan.SetGoalConfig( configuration ) ;
}

Configuration PL_Range_Sensor::GenerateRandomConfig () const
{
  int dof = GetStartConfig().DOF() ;
  Configuration returnme ;
  returnme.SetLength( dof ) ;
  
  //TRACE("\nPL_Range_Sensor::GenerateRandomConfig");
  
  //a chance for intermediate config to be set to goal config
  if (rand() > RAND_MAX*0.8)
  {
    //TRACE("\nIntermediate config set to goal config");
    return GetGoalConfig();
  }
  
  //generate a random value for all joint variables
  for( int i = 0; i < dof; i++ )
  {
    double max = collisionDetector->JointMax( i ) ;
    double min = collisionDetector->JointMin( i ) ;
    double random = double( rand() ) / RAND_MAX ; //this is a random number between [0..1]
    returnme[ i ] = min + ( max - min ) * random ;
  }
  
  return returnme ;
}

bool PL_Range_Sensor::DrawExplicit () const
{
        Semaphore semaphore( this->guid );
        semaphore.Lock();
        CD_Range_Sensor* cdRangeSensor = dynamic_cast< CD_Range_Sensor* >( this->collisionDetector );
        //use dataFuser to display contents of collision detector
        cdRangeSensor->dataFuser.GL_Display_Octree();
        semaphore.Unlock();
        return true;
};

---