step_controller_test_plugin.cpp
Go to the documentation of this file.
00001 #include <vigir_step_control/step_controller_test_plugin.h>
00002 
00003 
00004 
00005 namespace vigir_step_control
00006 {
00007 StepControllerTestPlugin::StepControllerTestPlugin()
00008   : StepControllerPlugin()
00009 {
00010 }
00011 
00012 StepControllerTestPlugin::~StepControllerTestPlugin()
00013 {
00014 }
00015 
00016 void StepControllerTestPlugin::initWalk()
00017 {
00018   // init feedback states
00019   msgs::ExecuteStepPlanFeedback feedback;
00020   feedback.header.stamp = ros::Time::now();
00021   feedback.last_performed_step_index = -2;
00022   feedback.currently_executing_step_index =-1;
00023   feedback.first_changeable_step_index = 0;
00024   setFeedbackState(feedback);
00025 
00026   next_step_needed_time_ = ros::Time::now();
00027 
00028   setState(ACTIVE);
00029 
00030   ROS_INFO("[StepControllerTestPlugin] Start fake execution.");
00031 }
00032 
00033 void StepControllerTestPlugin::preProcess(const ros::TimerEvent& event)
00034 {
00035   StepControllerPlugin::preProcess(event);
00036 
00037   if (getState() != ACTIVE)
00038     return;
00039 
00040   // fake succesful execution of single step
00041   if (next_step_needed_time_ <= ros::Time::now())
00042   {
00043     msgs::ExecuteStepPlanFeedback feedback = getFeedbackState();
00044 
00045     feedback.header.stamp = ros::Time::now();
00046     feedback.last_performed_step_index++;
00047 
00048     // check for successful execution of queue
00049     if (step_queue_->lastStepIndex() == feedback.last_performed_step_index)
00050     {
00051       ROS_INFO("[StepControllerTestPlugin] Fake execution finished.");
00052 
00053       feedback.currently_executing_step_index = -1;
00054       feedback.first_changeable_step_index = -1;
00055       setFeedbackState(feedback);
00056 
00057       step_queue_->reset();
00058       updateQueueFeedback();
00059 
00060       setState(FINISHED);
00061     }
00062     // otherwise trigger fake execution of next step
00063     else
00064     {
00065       feedback.currently_executing_step_index++;
00066       feedback.first_changeable_step_index++;
00067       setFeedbackState(feedback);
00068 
00069       setNextStepIndexNeeded(feedback.currently_executing_step_index);
00070     }
00071   }
00072 }
00073 
00074 bool StepControllerTestPlugin::executeStep(const msgs::Step& step)
00075 {
00076   next_step_needed_time_ = ros::Time::now() + ros::Duration(1.0 + step.step_duration);
00077   ROS_INFO("[StepControllerTestPlugin] Fake execution of step %i", step.step_index);
00078   return true;
00079 }
00080 } // namespace
00081 
00082 #include <pluginlib/class_list_macros.h>
00083 PLUGINLIB_EXPORT_CLASS(vigir_step_control::StepControllerTestPlugin, vigir_step_control::StepControllerPlugin)


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