step_controller.h
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2018, Alexander Stumpf, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Simulation, Systems Optimization and Robotics
13 // group, TU Darmstadt nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
28 
29 #ifndef STEP_CONTROLLER_H__
30 #define STEP_CONTROLLER_H__
31 
32 #include <ros/ros.h>
33 
35 
36 #include <vigir_pluginlib/plugin_manager.h>
37 
38 #include <vigir_footstep_planning_msgs/footstep_planning_msgs.h>
39 #include <vigir_footstep_planning_plugins/plugins/step_plan_msg_plugin.h>
40 
42 
43 
44 
46 {
47 using namespace vigir_footstep_planning_msgs;
48 
51 
53 {
54 public:
55  // typedefs
58 
64  virtual ~StepController();
65 
72  bool initialize(ros::NodeHandle& nh, bool auto_spin = true);
73 
82  bool initialize(ros::NodeHandle& nh, StepControllerPlugin::Ptr step_controller_plugin, vigir_footstep_planning::StepPlanMsgPlugin::Ptr step_plan_msg_plugin, bool auto_spin = true);
83 
90  template<typename T>
91  bool loadPlugin(const std::string& plugin_name, boost::shared_ptr<T>& plugin)
92  {
93  UniqueLock lock(controller_mutex_);
94 
95  if (step_controller_plugin_ && step_controller_plugin_->getState() == ACTIVE)
96  {
97  ROS_ERROR("[StepController] Cannot replace plugin due to active footstep execution!");
98  return false;
99  }
100 
101  if (!vigir_pluginlib::PluginManager::addPluginByName(plugin_name))
102  {
103  ROS_ERROR("[StepController] Could not load plugin '%s'!", plugin_name.c_str());
104  return false;
105  }
106  else if (!vigir_pluginlib::PluginManager::getPlugin(plugin))
107  {
108  ROS_ERROR("[StepController] Could not obtain plugin '%s' from plugin manager!", plugin_name.c_str());
109  return false;
110  }
111  else
112  ROS_INFO("[StepController] Loaded plugin '%s'.", plugin_name.c_str());
113 
114  return true;
115  }
116 
122  void executeStepPlan(const msgs::StepPlan& step_plan);
123 
127  void update(const ros::TimerEvent& event = ros::TimerEvent());
128 
129 protected:
133  void publishFeedback() const;
134 
136  vigir_footstep_planning::StepPlanMsgPlugin::Ptr step_plan_msg_plugin_;
137 
138  // mutex to ensure thread safeness
139  boost::shared_mutex controller_mutex_;
140 
142 
143  // subscriber
144  void loadStepPlanMsgPlugin(const std_msgs::StringConstPtr& plugin_name);
145  void loadStepControllerPlugin(const std_msgs::StringConstPtr& plugin_name);
146  void executeStepPlan(const msgs::StepPlanConstPtr& step_plan);
147 
148  // action server calls
149  void executeStepPlanAction(ExecuteStepPlanActionServerPtr as);
150  void executePreemptionAction(ExecuteStepPlanActionServerPtr as);
151 
152  // subscriber
156 
157  // publisher
159 
160  // action servers
162 
163  // timer for updating periodically
165 };
166 }
167 
168 #endif
ros::Subscriber load_step_controller_plugin_sub_
boost::shared_ptr< ExecuteStepPlanActionServer > execute_step_plan_as_
ROSCONSOLE_DECL void initialize()
ros::Subscriber load_step_plan_msg_plugin_sub_
bool loadPlugin(const std::string &plugin_name, boost::shared_ptr< T > &plugin)
Loads plugin with specific name to be used by the controller. The name should be configured in the pl...
vigir_footstep_planning::StepPlanMsgPlugin::Ptr step_plan_msg_plugin_
boost::shared_ptr< const StepController > ConstPtr
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
StepControllerPlugin::Ptr step_controller_plugin_
#define ROS_INFO(...)
boost::shared_ptr< StepController > Ptr
boost::unique_lock< Mutex > UniqueLock
Definition: step_queue.h:45
actionlib::SimpleActionServer< msgs::ExecuteStepPlanAction > ExecuteStepPlanActionServer
#define ROS_ERROR(...)
boost::shared_ptr< ExecuteStepPlanActionServer > ExecuteStepPlanActionServerPtr


vigir_step_control
Author(s): Alexander Stumpf
autogenerated on Mon Jun 10 2019 15:44:47