Orca Robotics

00001 /*
00002  * Orca-Robotics Project: Components for robotics 
00003  *               http://orca-robotics.sf.net/
00004  * Copyright (c) 2004-2008 Alex Brooks, Alexei Makarenko, Tobias Kaupp
00005  *
00006  * This copy of Orca is licensed to you under the terms described in
00007  * the LICENSE file included in this distribution.
00008  *
00009  */
00010 
00011 #ifndef SEGWAYRMP_HARDWARE_THREAD_H
00012 #define SEGWAYRMP_HARDWARE_THREAD_H
00013 
00014 #include <memory>
00015 #include <gbxsickacfr/gbxiceutilacfr/subsystemthread.h>
00016 #include <orcaice/context.h>
00017 #include <hydrointerfaces/segwayrmp.h>
00018 #include <hydrodll/dynamicload.h>
00019 
00020 #include <gbxsickacfr/gbxiceutilacfr/store.h>
00021 #include <orcarobotdriverutil/statemachine.h>
00022 #include <gbxsickacfr/gbxiceutilacfr/timer.h>
00023 #include "estop.h"
00024 
00025 namespace segwayrmp {
00026 
00027 
00028 //
00029 // @brief The thread that runs the driver
00030 //
00031 // @author Alex Brooks
00032 //
00033 class HwThread : public gbxsickacfr::gbxiceutilacfr::SubsystemThread
00034 {
00035 
00036 public: 
00037 
00038     // this is a subset of configuration parameters which is checked by the hardware driver
00039     struct Config
00040     {
00041         double maxForwardSpeed;
00042         double maxReverseSpeed;
00043         double maxTurnrate;
00044         double maxLateralAcceleration;
00045     };
00046 
00047     // these config settings are checked and possibly limited based on hardware capabilities
00048     HwThread( Config& config, const orcaice::Context &context );
00049     
00050     // Inherited from SubsystemThread
00051     virtual void walk();
00052 
00053     // local interface, used by NetThread
00054 
00055     // used by NetThread to pass commands, events and get data
00056     void setCommand( const hydrointerfaces::SegwayRmp::Command &command );
00057 
00058     int getData( hydrointerfaces::SegwayRmp::Data &data, int timeoutMs )
00059         {
00060             return dataStore_.getNext( data, timeoutMs );
00061         }
00062 
00063 private: 
00064 
00065     // Keeps trying until success or isStopping()
00066     void enableDriver();
00067     void writeCommand( hydrointerfaces::SegwayRmp::Command &command );
00068 
00069     // Checks with the driver, and sets the stateMachine_ accordingly.
00070     void checkStatus();
00071 
00072     // Faults can be detected in either read or write threads: have to be careful.
00073     orcarobotdriverutil::StateMachine stateMachine_;
00074 
00075     // Stores the data most recently received from the hardware
00076     gbxsickacfr::gbxiceutilacfr::Store<hydrointerfaces::SegwayRmp::Data>    dataStore_;
00077     // Stores incoming commands
00078     gbxsickacfr::gbxiceutilacfr::Store<hydrointerfaces::SegwayRmp::Command> commandStore_;
00079 
00080     bool isMotionEnabled_;
00081     bool driveInReverse_;
00082 
00083     std::auto_ptr<EStop> eStop_;
00084 
00085     // Looks for late writes (which will cause timeouts in the segway)
00086     gbxsickacfr::gbxiceutilacfr::Timer writeTimer_;
00087 
00088     // The library that contains the driver factory (must be declared first so it's destructed last!!!)
00089     std::auto_ptr<hydrodll::DynamicallyLoadedLibrary> driverLib_;
00090     // Generic driver for the hardware
00091     std::auto_ptr<hydrointerfaces::SegwayRmp> driver_;
00092 
00093     orcaice::Context context_;
00094 };
00095 
00096 } // namespace
00097 
00098 #endif
segwayrmp/hwthread.h
