Go to the documentation of this file.00001 #include <vigir_step_control/step_controller.h>
00002
00003 #include <vigir_generic_params/parameter_manager.h>
00004
00005
00006
00007 namespace vigir_step_control
00008 {
00009 StepController::StepController(ros::NodeHandle& nh, bool auto_spin)
00010 {
00011 vigir_pluginlib::PluginManager::addPluginClassLoader<vigir_footstep_planning::StepPlanMsgPlugin>("vigir_footstep_planning_plugins", "vigir_footstep_planning::StepPlanMsgPlugin");
00012 vigir_pluginlib::PluginManager::addPluginClassLoader<StepControllerPlugin>("vigir_step_control", "vigir_step_control::StepControllerPlugin");
00013
00014
00015 loadPlugin(nh.param("step_plan_msg_plugin", std::string("step_plan_msg_plugin")), step_plan_msg_plugin_);
00016
00017
00018 loadPlugin(nh.param("step_controller_plugin", std::string("step_controller_test_plugin")), step_controller_plugin_);
00019 if (step_controller_plugin_)
00020 step_controller_plugin_->setStepPlanMsgPlugin(step_plan_msg_plugin_);
00021
00022
00023 load_step_plan_msg_plugin_sub_ = nh.subscribe("load_step_plan_msg_plugin", 1, &StepController::loadStepPlanMsgPlugin, this);
00024 load_step_controller_plugin_sub_ = nh.subscribe("load_step_controller_plugin", 1, &StepController::loadStepControllerPlugin, this);
00025 execute_step_plan_sub_ = nh.subscribe("execute_step_plan", 1, &StepController::executeStepPlan, this);
00026
00027
00028 planning_feedback_pub_ = nh.advertise<msgs::ExecuteStepPlanFeedback>("execute_feedback", 1, true);
00029
00030
00031 execute_step_plan_as_.reset(new ExecuteStepPlanActionServer(nh, "execute_step_plan", false));
00032 execute_step_plan_as_->registerGoalCallback(boost::bind(&StepController::executeStepPlanAction, this, boost::ref(execute_step_plan_as_)));
00033 execute_step_plan_as_->registerPreemptCallback(boost::bind(&StepController::executePreemptionAction, this, boost::ref(execute_step_plan_as_)));
00034
00035
00036 execute_step_plan_as_->start();
00037
00038
00039 if (auto_spin)
00040 update_timer_ = nh.createTimer(nh.param("rate", 10.0), &StepController::update, this);
00041 }
00042
00043 StepController::~StepController()
00044 {
00045 }
00046
00047 void StepController::executeStepPlan(const msgs::StepPlan& step_plan)
00048 {
00049 boost::unique_lock<boost::shared_mutex> lock(controller_mutex_);
00050
00051 if (!step_controller_plugin_)
00052 {
00053 ROS_ERROR("[StepController] executeStepPlan: No step_controller_plugin available!");
00054 return;
00055 }
00056
00057
00058 if (step_plan.steps.empty())
00059 step_controller_plugin_->stop();
00060 else
00061 step_controller_plugin_->updateStepPlan(step_plan);
00062 }
00063
00064 void StepController::update(const ros::TimerEvent& event)
00065 {
00066 boost::unique_lock<boost::shared_mutex> lock(controller_mutex_);
00067
00068 if (!step_controller_plugin_)
00069 {
00070 ROS_ERROR_THROTTLE(5.0, "[StepController] update: No step_controller_plugin available!");
00071 return;
00072 }
00073
00074
00075
00076
00077 StepControllerState state = step_controller_plugin_->getState();
00078
00079
00080 step_controller_plugin_->preProcess(event);
00081
00082
00083 step_controller_plugin_->process(event);
00084
00085 lock.unlock();
00086
00087
00088 publishFeedback();
00089
00090
00091 switch (state)
00092 {
00093 case FINISHED:
00094 if (execute_step_plan_as_->isActive())
00095 execute_step_plan_as_->setSucceeded(msgs::ExecuteStepPlanResult());
00096 break;
00097
00098 case FAILED:
00099 if (execute_step_plan_as_->isActive())
00100 execute_step_plan_as_->setAborted(msgs::ExecuteStepPlanResult());
00101 break;
00102
00103 default:
00104 break;
00105 }
00106
00107
00108 step_controller_plugin_->postProcess(event);
00109 }
00110
00111 void StepController::publishFeedback() const
00112 {
00113 if (step_controller_plugin_->getState() != READY)
00114 {
00115 const msgs::ExecuteStepPlanFeedback& feedback = step_controller_plugin_->getFeedbackState();
00116
00117
00118 planning_feedback_pub_.publish(feedback);
00119
00120 if (execute_step_plan_as_->isActive())
00121 execute_step_plan_as_->publishFeedback(feedback);
00122 }
00123 }
00124
00125
00126
00127 void StepController::loadStepPlanMsgPlugin(const std_msgs::StringConstPtr& plugin_name)
00128 {
00129 loadPlugin(plugin_name->data, step_plan_msg_plugin_);
00130
00131 if (step_controller_plugin_)
00132 step_controller_plugin_->setStepPlanMsgPlugin(step_plan_msg_plugin_);
00133 }
00134
00135 void StepController::loadStepControllerPlugin(const std_msgs::StringConstPtr& plugin_name)
00136 {
00137 loadPlugin(plugin_name->data, step_controller_plugin_);
00138
00139 if (step_controller_plugin_)
00140 step_controller_plugin_->setStepPlanMsgPlugin(step_plan_msg_plugin_);
00141 }
00142
00143 void StepController::executeStepPlan(const msgs::StepPlanConstPtr& step_plan)
00144 {
00145 executeStepPlan(*step_plan);
00146 }
00147
00148
00149
00150 void StepController::executeStepPlanAction(ExecuteStepPlanActionServerPtr& as)
00151 {
00152 const msgs::ExecuteStepPlanGoalConstPtr& goal(as->acceptNewGoal());
00153
00154
00155 if (as->isPreemptRequested())
00156 {
00157 as->setPreempted();
00158 return;
00159 }
00160
00161 executeStepPlan(goal->step_plan);
00162 }
00163
00164 void StepController::executePreemptionAction(ExecuteStepPlanActionServerPtr& as)
00165 {
00166 if (as->isActive())
00167 as->setPreempted();
00168
00169
00170 }
00171 }