collisiondetectors/CD_rangesensor/CD_Range_Sensor.cpp

Go to the documentation of this file.

// CD_Range_Sensor
#include "CD_Range_Sensor.h"

//#include <stdlib.h>


CD_Range_Sensor::CD_Range_Sensor(Universe& universe)
//note: is CD_BasicStyle the appropriate parent???
//I hope this doesn't do anything with the universe because it's not the universe
//we want to use for the sensor based collison dectection
: 
        CollisionDetectorBase( universe ),
        CD_BasicStyle( universe ),
        CD_LinearContinuous( universe ),
        CD_Linear( universe )
{
        //TRACE("\nCD_Range_Sensor::CD_Range_Sensor(u) constructor");
        int i,numfreeblocks;
        unsigned int randseed;
        bool loadfromfile;
        
        //  It is REQUIRED that the universe passed into the object
        //  does not move for the life of the CD_Range_Sensor object.
        //  (used in the take_picture method)
        up = &universe;
        char marker[ 300 ] = "" ;
        cubic worldextents; //the maximum volume of scannable space around robot
        cubic newblocksize; //the assumed free space around robot
                
        ifstream is(CD_Range_Sensor_Settings_Filename);
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#maxlevel" ) == 0 ) ;
        is >> dataFuser.maxlevel ;
        assert (dataFuser.maxlevel > 0);
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#octree_display_mode" ) == 0 ) ;
        //Only display octree in mesh form in multithreaded mode
        is >> dataFuser.octree_display_mode ;
        if (isMultiThreaded == false)
        {
                dataFuser.octree_display_mode = octree_nodisplay;
        }

  eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#track_sensed_obstacles" ) == 0 ) ;
        is >> dataFuser.track_sensed_obstacles ;
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#savetofile" ) == 0 ) ;
        is >> savetofile ;
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#loadfromfile" ) == 0 ) ;
        is >> loadfromfile ;
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#worldextents" ) == 0 ) ;
        is >> dataFuser.scalefactor ;
        //constant 128.0 due to octree algorithim hard coded limit of world extents
        dataFuser.scalefactor = float(128.0/dataFuser.scalefactor);
        
        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#expected_vertexes" ) == 0 ) ;
        is >> dataFuser.expected_vertexes ;
        dataFuser.expected_facets = dataFuser.expected_vertexes/2;
        dataFuser.expected_squarevectors = dataFuser.expected_vertexes*2/3;

        eatwhite( is ) ;
        is.getline( marker, 300, ' ' ) ;
        assert( strcmp( marker, "#randseed" ) == 0 ) ;
        is >> randseed ;
        srand(randseed);
        
        if (loadfromfile)
        {
                dataFuser.Load_State(CD_Range_Sensor_Octree_Filename);
        } else
        {
                eatwhite( is ) ;
                is.getline( marker, 300, ' '  ) ;
                assert( strcmp( marker, "#worldlowpoint" ) == 0 ) ;
                is >> worldextents.x0;
                is >> worldextents.y0;
                is >> worldextents.z0;
        
                eatwhite( is ) ;
                is.getline( marker, 300, ' '  ) ;
                assert( strcmp( marker, "#worldhighpoint" ) == 0 ) ;
                is >> worldextents.x1;
                is >> worldextents.y1;
                is >> worldextents.z1;
        
                eatwhite( is ) ;
                is.getline( marker, 300, ' ' ) ;
                assert( strcmp( marker, "#numfreeblocks" ) == 0 ) ;
                is >> numfreeblocks ;
                assert (numfreeblocks > 0);
        
                for (i= 0; i < numfreeblocks; i++)
                {
                        eatwhite( is ) ;
                        is.getline( marker, 300, ' '  ) ;
                        assert( strcmp( marker, "#freelowpoint" ) == 0 ) ;
                        is >> newblocksize.x0;
                        is >> newblocksize.y0;
                        is >> newblocksize.z0;
                        
                        eatwhite( is ) ;
                        is.getline( marker, 300, ' '  ) ;
                        assert( strcmp( marker, "#freehighpoint" ) == 0 ) ;
                        is >> newblocksize.x1;
                        is >> newblocksize.y1;
                        is >> newblocksize.z1;
                        
                        dataFuser.Add_Block( worldextents, newblocksize );
                }
        }
        
        is.close();
        
        //grab a universe with only the robot in it:
    
        allorigEntities.clear();
        allorigEntities = universe.GetAllEntities() ;
        
        //note: the universe copy constructor seems unreliable
        //robot_universe = universe;
        
        //  Take the passed in Universe object, and scan through to the find all
        //  RobotBase objects.
        robot_universe.Empty();
        int size = allorigEntities.size() ;
        for( i = 0; i < size; i++ )
        {
                const R_OpenChain* r_openchain = dynamic_cast< const R_OpenChain* >( allorigEntities[ i ] ) ;
                if( r_openchain != NULL )
                {
                        //  Then, using AddEntity() add them to a custom universe object.  This will
                        //  have the effect of copying all the robot geometries but none of geometries
                        //  for the rest of the world.
                        Entity* addme = allorigEntities[ i ]->Clone() ;
                        addme->SetFrameManager( robot_universe.GetFrameManager() ) ;
                        robot_universe.AddEntity( addme );
                        //int size = robot_universe.GetFrameManager()->GetNumberOfFrames() ;
                        delete addme;
                        //TRACE("\nR_OpenChain found.");
                }
        }
  lv = NULL;
  lv_obstacle = NULL;
        
        //always create white octree node mesh in beginning in case user wants to use it later
        dataFuser.Convert(lv, lv_obstacle, robot_universe, false);

}

CD_Range_Sensor::CD_Range_Sensor (const CD_Range_Sensor& right)
//I hope this doesn't do anything with the universe because it's not the universe
//we want to use for the sensor based collison dectection
: 
        CollisionDetectorBase( right ),
        CD_BasicStyle( right ),
        CD_LinearContinuous( right ),
        CD_Linear( right )
{
        this->dataFuser = right.dataFuser;
        this->savetofile = right.savetofile;
        this->isMultiThreaded = right.isMultiThreaded;
        this->allorigEntities = right.allorigEntities;
        this->lv = dynamic_cast<CD_Vcollide *>(right.lv->Clone());
        this->lv_obstacle = dynamic_cast<CD_Vcollide *>(right.lv_obstacle->Clone());
        
        //  Take the passed in right Universe object, and scan through to the find all
        //  RobotBase objects.
        
        //note: the universe copy constructor seems unreliable
        //      this->robot_universe = right.robot_universe;

        std::vector< Entity* > tempEntities;
        tempEntities.clear();
        tempEntities = right.robot_universe.GetAllEntities() ;
        
        this->robot_universe.Empty();
        int size = tempEntities.size() ;
        for( int i = 0; i < size; i++ )
        {
                const R_OpenChain* r_openchain = dynamic_cast< const R_OpenChain* >( tempEntities[ i ] ) ;
                if( r_openchain != NULL )
                {
                        //  Then, using AddEntity() add them to a custom universe object.  This will
                        //  have the effect of copying all the robot geometries but none of geometries
                        //  for the rest of the world.
                        Entity* addme = tempEntities[ i ]->Clone() ;
                        addme->SetFrameManager( this->robot_universe.GetFrameManager() ) ;
                        this->robot_universe.AddEntity( addme );
                        delete addme;
                }
        }
        
}

//note: the following was just copied and converted from CD_Vcollide()
CollisionDetectorBase* CD_Range_Sensor::Clone () const
{
        CD_Range_Sensor* newCD_J = new CD_Range_Sensor( *this ) ;
        CD_InterfaceToCollisionQueries* CDitc = dynamic_cast< CD_Bool* >( newCD_J ) ;
        CollisionDetectorBase* returnMe = dynamic_cast< CollisionDetectorBase* >( CDitc );
        return returnMe ;
}

CD_Range_Sensor::~CD_Range_Sensor()
{
        //TRACE("\nCD_Range_Sensor::~CD_Range_Sensor()");

        allorigEntities.clear();

        //delete local vcollide universe
        delete lv;
  delete lv_obstacle;
}


Range_Sensor* CD_Range_Sensor::FindFirstCamera (Entity* entity)
{
        //TRACE("\nCD_Range_Sensor::FindFirstCamera()");
        
        Range_Sensor* camera;
        
        ObjectGroup* group = dynamic_cast< ObjectGroup* >( entity ) ;
        if( group != NULL )
        {
                for( int i = 0; i < group->Size(); i++ )
                {
                        const ObjectBase* addme = ( *group )[ i ] ;
                        //recursively look into objects for something a sensor can detect
                        camera = FindFirstCamera( dynamic_cast< Entity *> (const_cast<ObjectBase*>(addme)) ) ;
                }
                return camera;
        }
        
        camera = dynamic_cast< Range_Sensor* >( entity ) ;
        if( camera != NULL )
        {
                //TRACE ("\ncamera found");
                //Make sure the pointer to allorigEntites is still valid. (original Universe cannot move)
                //only display 2D map when not multithreaded
                camera->Take_Picture(allorigEntities, !isMultiThreaded);
                dataFuser.Update(camera);

                return camera;
        }
        //TRACE("\nFailed to find camera in bottom of objectgroup");
        return NULL;
}

void CD_Range_Sensor::Take_Picture(Configuration newconfig)
{
        //TRACE("\nCD_Range_Sensor::Take_Picture()");
        //probe the first camera you find.
        
        //delete the huge lv var as soon as possible (they will always be replaced here)
        delete lv;
  delete lv_obstacle;
        
        Configuration oldconfig = up->GetConfiguration();
        //temporarily change the universe's configuration to the new "planning stage" one
        up->SetConfiguration(newconfig);
        
        Range_Sensor* camera;
        
        int size = allorigEntities.size() ;
        
        for( int i = 0; i < size; i++ )
        {
                Entity* entity = allorigEntities[ i ] ;
                //look for all cameras, and have them all take pictures
                camera = FindFirstCamera (entity);
                if (camera != NULL)
                {
                        //TRACE("\nCamera found in Universe");
                }
        }
                
        //for debugging
        if (savetofile)
        {
                dataFuser.Save_State(CD_Range_Sensor_Octree_Filename);
        }
        
#ifdef WIN32
        dataFuser.cWindow_ptr = cWindow_ptr;
    dataFuser.hglrc_var = hglrc_var;
#endif
        dataFuser.showconfig = newconfig;

        //next time local universe lu is used (possibly to display onscreen), it will have the right robot config
        //always create white octree node mesh in beginning in case user wants to see it later
        dataFuser.Convert(lv, lv_obstacle, robot_universe, true);
        
        //make sure we're not damaging anything else by resetting the old configuration.
        //(not sure if this is neccesscary)
        up->SetConfiguration(oldconfig);
        
}


bool CD_Range_Sensor::IsInterfering (const Configuration& config)
{
        //TRACE("\nCD_Range_Sensor::IsInterfering");
  bool isinterfere = lv->IsInterfering (config);

  if (isinterfere == true)
  {
    if ( (lv_obstacle != NULL) && (lv_obstacle->IsInterfering (config)) )
    {
    }
  }
        
        //using the CD_vcollide, determine collision with this config
        return( isinterfere );  
}

---