Orca Robotics

00001 #ifndef SIMULATOR_H
00002 #define SIMULATOR_H
00003 
00004 #include <hydrosim2d/hydrosim2d.h>
00005 #include <orca/pathfollower2d.h>
00006 #include <orcasim2d/orcasim2d.h>
00007 #include <memory>
00008 #include <orcaifaceimpl/ogmapImpl.h>
00009 
00010 namespace localnav {
00011 
00012 //
00013 // @author Alex Brooks
00014 //
00015 class Simulator
00016 {
00017 
00018 public: 
00019 
00020     struct Config {
00021         double maxLateralAcceleration;
00022         bool   checkLateralAcceleration;
00023         bool   checkDifferentialConstraints;
00024         bool   checkVelocityConstraints;
00025         bool   useRoom;
00026         bool   batchMode;
00027         int    numIterationsBatch;
00028     };
00029 
00030     Simulator( const orcaice::Context         &context,
00031                const hydroogmap::OgMap        &ogMap,
00032                const orca::PathFollower2dData &testPath,
00033                const Config                   &config );
00034 
00035     // This is the trigger to advance the simulator one step.
00036     void act( const hydronavutil::Velocity &cmd );
00037 
00038     void checkProgress( bool &pathCompleted, bool &pathFailed );
00039 
00040     void getObsRanges( std::vector<float> &obsRanges )
00041         { rangeScannerSimulator_->getRangesFromPose( vehicleSimulator_->pose(), obsRanges ); }
00042 
00043     hydronavutil::Pose pose() const
00044         { return vehicleSimulator_->pose(); }
00045     hydronavutil::Velocity velocity() const
00046         { return vehicleSimulator_->velocity(); }
00047 
00048     orca::VehicleDescription vehicleDescription() const
00049         { return vehicleDescr_; }
00050 
00051     const orca::RangeScanner2dDescription& rangeScanner2dDescription() const
00052         { return scannerDescr_; }
00053     
00054     orca::Time time() const;
00055 
00056     const hydrosim2d::VehicleSimulator &vehicleSimulator() const { return *vehicleSimulator_; }
00057     const hydroogmap::OgMap &ogMap() const { return ogMap_; }
00058 
00059 private: 
00060 
00061     void setupInterfaces( const hydrosim2d::VehicleSimulator::Config &vehicleSimConfig,
00062                           const hydrosim2d::RangeScannerSimulator::Config rangeScanSimConfig );
00063 
00064     std::auto_ptr<orcasim2d::PosePublisher>      posePublisher_;
00065     std::auto_ptr<orcasim2d::RangeScanPublisher> rangeScanPublisher_;
00066     orcaifaceimpl::OgMapImplPtr                  ogMapInterface_;
00067 
00068     orca::RangeScanner2dDescription scannerDescr_;
00069     orca::VehicleDescription        vehicleDescr_;
00070 
00071     hydroogmap::OgMap        ogMap_;
00072     orca::PathFollower2dData testPath_;
00073 
00074     int    iterationNum_;
00075     int    wpI_;
00076 
00077     std::auto_ptr<hydrosim2d::VehicleSimulator>      vehicleSimulator_;
00078     std::auto_ptr<hydrosim2d::RangeScannerSimulator> rangeScannerSimulator_;
00079 
00080     const Config config_;
00081 
00082     orcaice::Context context_;
00083 };
00084 
00085 std::string toString( const Simulator::Config &c );
00086 
00087 }
00088 
00089 #endif
simulator.h
