quadrotor_interface.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2013, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
00030 #define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
00031 
00032 #include <hardware_interface/internal/hardware_resource_manager.h>
00033 
00034 #include <hector_quadrotor_controller/handles.h>
00035 
00036 #include <map>
00037 #include <list>
00038 
00039 namespace hector_quadrotor_controller {
00040 
00041 using namespace hardware_interface;
00042 
00043 class QuadrotorInterface : public HardwareInterface
00044 {
00045 public:
00046   QuadrotorInterface();
00047   virtual ~QuadrotorInterface();
00048 
00049   virtual PoseHandlePtr getPose()                 { return PoseHandlePtr(); }
00050   virtual TwistHandlePtr getTwist()               { return TwistHandlePtr(); }
00051   virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(); }
00052   virtual ImuHandlePtr getSensorImu()             { return ImuHandlePtr(); }
00053   virtual MotorStatusHandlePtr getMotorStatus()   { return MotorStatusHandlePtr(); }
00054 
00055   virtual bool getMassAndInertia(double &mass, double inertia[3]) { return false; }
00056 
00057   template <typename HandleType> boost::shared_ptr<HandleType> getHandle()
00058   {
00059     return boost::shared_ptr<HandleType>(new HandleType(this));
00060   }
00061 
00062   template <typename HandleType> boost::shared_ptr<HandleType> addInput(const std::string& name)
00063   {
00064     boost::shared_ptr<HandleType> input = getInput<HandleType>(name);
00065     if (input) return input;
00066 
00067     // create new input handle
00068     input.reset(new HandleType(this, name));
00069     inputs_[name] = input;
00070 
00071     // connect to output of same name
00072     if (outputs_.count(name)) {
00073       boost::shared_ptr<HandleType> output = boost::dynamic_pointer_cast<HandleType>(outputs_.at(name));
00074       output->connectTo(*input);
00075     }
00076 
00077     return input;
00078   }
00079 
00080   template <typename HandleType> boost::shared_ptr<HandleType> addOutput(const std::string& name)
00081   {
00082     boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
00083     if (output) return output;
00084 
00085     // create new output handle
00086     output.reset(new HandleType(this, name));
00087     outputs_[name] = output;
00088     *output = output->ownData(new typename HandleType::ValueType());
00089 
00090     //claim resource
00091     claim(name);
00092 
00093     // connect to output of same name
00094     if (inputs_.count(name)) {
00095       boost::shared_ptr<HandleType> input = boost::dynamic_pointer_cast<HandleType>(inputs_.at(name));
00096       output->connectTo(*input);
00097     }
00098 
00099     return output;
00100   }
00101 
00102   template <typename HandleType> boost::shared_ptr<HandleType> getOutput(const std::string& name) const
00103   {
00104     if (!outputs_.count(name)) return boost::shared_ptr<HandleType>();
00105     return boost::static_pointer_cast<HandleType>(outputs_.at(name));
00106   }
00107 
00108   template <typename HandleType> boost::shared_ptr<HandleType> getInput(const std::string& name) const
00109   {
00110     if (!inputs_.count(name)) return boost::shared_ptr<HandleType>();
00111     return boost::static_pointer_cast<HandleType>(inputs_.at(name));
00112   }
00113 
00114   template <typename HandleType> typename HandleType::ValueType const* getCommand(const std::string& name) const
00115   {
00116     boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
00117     if (!output || !output->connected()) return 0;
00118     return &(output->command());
00119   }
00120 
00121   virtual const Pose *getPoseCommand() const;
00122   virtual const Twist *getTwistCommand() const;
00123   virtual const Wrench *getWrenchCommand() const;
00124   virtual const MotorCommand *getMotorCommand() const;
00125 
00126   bool enabled(const CommandHandle *handle) const;
00127   bool start(const CommandHandle *handle);
00128   void stop(const CommandHandle *handle);
00129 
00130   void disconnect(const CommandHandle *handle);
00131 
00132 private:
00133 //  typedef std::list< CommandHandlePtr > HandleList;
00134 //  std::map<const std::type_info *, HandleList> inputs_;
00135 //  std::map<const std::type_info *, HandleList> outputs_;
00136   std::map<std::string, CommandHandlePtr> inputs_;
00137   std::map<std::string, CommandHandlePtr> outputs_;
00138   std::map<std::string, const CommandHandle *> enabled_;
00139 };
00140 
00141 } // namespace hector_quadrotor_controller
00142 
00143 #endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H


hector_quadrotor_controller
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:49:53