step_controller_plugin.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2018, Alexander Stumpf, 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 Simulation, Systems Optimization and Robotics
00013 //       group, 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 STEP_CONTROLLER_PLUGIN_H__
00030 #define STEP_CONTROLLER_PLUGIN_H__
00031 
00032 #include <ros/ros.h>
00033 
00034 #include <vigir_pluginlib/plugin.h>
00035 
00036 #include <vigir_footstep_planning_plugins/plugins/step_plan_msg_plugin.h>
00037 
00038 #include <vigir_step_control/step_queue.h>
00039 
00040 
00041 
00042 namespace vigir_step_control
00043 {
00044 using namespace vigir_footstep_planning_msgs;
00045 
00046 enum StepControllerState
00047 {
00048   NOT_READY = msgs::ExecuteStepPlanFeedback::NOT_READY,
00049   READY     = msgs::ExecuteStepPlanFeedback::READY,
00050   ACTIVE    = msgs::ExecuteStepPlanFeedback::ACTIVE,
00051   PAUSED    = msgs::ExecuteStepPlanFeedback::PAUSED,
00052   FINISHED  = msgs::ExecuteStepPlanFeedback::FINISHED,
00053   FAILED    = msgs::ExecuteStepPlanFeedback::FAILED
00054 };
00055 
00056 std::string toString(const StepControllerState& state);
00057 
00058 
00068 class StepControllerPlugin
00069   : public vigir_pluginlib::Plugin
00070 {
00071 public:
00072   // typedefs
00073   typedef boost::shared_ptr<StepControllerPlugin> Ptr;
00074   typedef boost::shared_ptr<const StepControllerPlugin> ConstPtr;
00075 
00076   StepControllerPlugin();
00077   virtual ~StepControllerPlugin();
00078 
00083   virtual void setStepPlanMsgPlugin(vigir_footstep_planning::StepPlanMsgPlugin::Ptr plugin);
00084 
00089   StepControllerState getState() const;
00090 
00095   int getNextStepIndexNeeded() const;
00096 
00101   int getLastStepIndexSent() const;
00102 
00107   const msgs::ExecuteStepPlanFeedback& getFeedbackState() const;
00108 
00112   void updateQueueFeedback();
00113 
00124   virtual bool updateStepPlan(const msgs::StepPlan& step_plan);
00125 
00130   virtual void initWalk() = 0;
00131 
00142   virtual void preProcess(const ros::TimerEvent& event);
00143 
00150   virtual void process(const ros::TimerEvent& event);
00151 
00155   virtual void postProcess(const ros::TimerEvent& event);
00156 
00162   virtual bool executeStep(const msgs::Step& step) = 0;
00163 
00167   virtual void stop();
00168 
00169 protected:
00174   virtual void reset();
00175 
00176   void setState(StepControllerState state);
00177 
00178   void setNextStepIndexNeeded(int index);
00179 
00180   void setLastStepIndexSent(int index);
00181 
00182   void setFeedbackState(const msgs::ExecuteStepPlanFeedback& feedback);
00183 
00184   StepQueue::Ptr step_queue_;
00185 
00186   vigir_footstep_planning::StepPlanMsgPlugin::Ptr step_plan_msg_plugin_;
00187 
00188   // mutex to ensure thread safeness
00189   mutable boost::shared_mutex plugin_mutex_;
00190 
00191 private:
00192   // current state of walk controller
00193   StepControllerState state_;
00194 
00195   // next step index needed by walk engine
00196   int next_step_index_needed_;
00197 
00198   // last step index sent to walk engine
00199   int last_step_index_sent_;
00200 
00201   // contains current feedback state; should be updated in each cycle
00202   msgs::ExecuteStepPlanFeedback feedback_state_;
00203 };
00204 }
00205 
00206 #endif


vigir_step_control
Author(s): Alexander Stumpf
autogenerated on Thu Jun 6 2019 21:13:33