multi_trigger_controller.cpp
Go to the documentation of this file.
00001 
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2009, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the Willow Garage nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <boost/format.hpp>
00037 
00038 #include "ethercat_trigger_controllers/multi_trigger_controller.h"
00039 #include "ros/console.h"
00040 #include "pluginlib/class_list_macros.h"
00041 
00042 PLUGINLIB_EXPORT_CLASS( controller::MultiTriggerController, pr2_controller_interface::Controller)
00043 
00044 using std::string;
00045 using namespace controller;
00046 
00047 MultiTriggerController::MultiTriggerController()
00048 {
00049   ROS_DEBUG("creating controller...");
00050 }
00051 
00052 MultiTriggerController::~MultiTriggerController()
00053 {
00054 }
00055 
00056 void MultiTriggerController::update()
00057 {
00058   int maxloops = 10; // @todo Workaround to avoid breaking realtime in response to #3274. Need to revamp things to fix this better.
00059   
00060   if (!config_.transitions.empty() && config_mutex_.try_lock())
00061   { // If we missed the lock, then hold current value for now.
00062     ros::Time cur_time = robot_->getTime();
00063 
00064     while (cur_time.toSec() >= transition_time_) // Usually only happens at most once per update.
00065     {
00066       if (!maxloops--)
00067       {
00068         //ROS_ERROR("MultiTriggerController exceeded iteration limit. Please report this to Blaise."); // @todo remove this
00069         break;
00070       }
00071 
00072       //ROS_INFO("hit %f %f %i", cur_time.toSec(), transition_time_, config_.transitions[transition_index_].value);
00073       // Do the transition
00074       digital_out_command_->data_ = config_.transitions[transition_index_].value;
00075       if (pubs_[transition_index_] && pubs_[transition_index_]->trylock())
00076       {
00077         pubs_[transition_index_]->msg_.stamp = cur_time;
00078         pubs_[transition_index_]->unlockAndPublish();
00079       }
00080       
00081       // Prepare for next transition
00082       if (++transition_index_ == config_.transitions.size())
00083       {
00084         transition_index_ = 0;
00085         transition_period_++;
00086       }
00087       transition_time_ = config_.transitions[transition_index_].time + config_.period * transition_period_ + config_.zero_offset;
00088     }
00089     config_mutex_.unlock();
00090   }
00091 }
00092 
00093 template <class T>
00094 static bool parseParamList(ros::NodeHandle nh, std::string param, std::vector<T> &rslt)
00095 {
00096   std::string param_value;
00097   nh.getParam(param, param_value);
00098   std::stringstream parser(param_value);
00099   T value;
00100   while (parser >> value)
00101     rslt.push_back(value);
00102     
00103   bool eof = parser.eof();
00104 
00105   if (!eof)
00106     ROS_ERROR("Error parsing '%s/%s' argument to MultiTriggerController.", nh.getNamespace().c_str(), param.c_str());
00107 
00108   return eof; // Returns true on success
00109 }
00110 
00111 bool MultiTriggerController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle& n)
00112 {
00113   node_handle_ = n;
00114 
00115   ROS_DEBUG("LOADING TRIGGER CONTROLLER NODE");
00116   //string prefix = config->Attribute("name");
00117   //ROS_DEBUG_STREAM("the prefix is "<<prefix);
00118 
00119   assert(robot);
00120   robot_=robot;
00121 
00122   ROS_DEBUG("MultiTriggerController::init starting");
00123 
00124   // Get the digital out name.
00125 
00126   if (!n.getParam("digital_output", digital_output_name_)){
00127     ROS_ERROR("MultiTriggerController was not given a digital_output parameter.");
00128     return false;
00129   }
00130 
00131   pr2_hardware_interface::DigitalOut *digital_out = robot_->model_->hw_->getDigitalOut(digital_output_name_);
00132   if (!digital_out)
00133   {
00134     ROS_ERROR("MultiTriggerController could not find digital output named \"%s\".",
00135         digital_output_name_.c_str());
00136     return false;
00137   }
00138 
00139   digital_out_command_ = &digital_out->command_;
00140 
00141   n.param("period", config_.period, 1.);
00142   n.param("zero_offset", config_.zero_offset, 0.);
00143 
00144   std::vector<string> topics;
00145   std::vector<double> times;
00146   std::vector<uint32_t> values;
00147   
00148   waveform_ = node_handle_.advertise<ethercat_trigger_controllers::MultiWaveform>("waveform", 1, true);
00149   
00150   if (parseParamList(n, "times", times) && parseParamList(n, "topics", topics) && parseParamList(n, "values", values))
00151   {
00152     if (times.size() != topics.size() || times.size() != values.size())
00153       ROS_ERROR("'topics', 'times' and 'values' parameters must have same length in %s. Ignoring initial settings.", 
00154           n.getNamespace().c_str());
00155     else
00156     {
00157       for (unsigned int i = 0; i < times.size(); i++)
00158       {
00159         ethercat_trigger_controllers::MultiWaveformTransition t;
00160         t.time = times[i];
00161         t.value = values[i];
00162         t.topic = topics[i];
00163         config_.transitions.push_back(t);
00164       }
00165 
00166       ethercat_trigger_controllers::SetMultiWaveform::Request req;
00167       req.waveform = config_;
00168       ethercat_trigger_controllers::SetMultiWaveform::Response dummy;
00169       setMultiWaveformSrv(req, dummy);
00170     }
00171   }
00172 
00173   set_waveform_handle_ = node_handle_.advertiseService("set_waveform", &MultiTriggerController::setMultiWaveformSrv, this);
00174 
00175   ROS_DEBUG("MultiTriggerController::init completed successfully.");
00176 
00177   return true;
00178 }
00179 
00180 bool MultiTriggerController::setMultiWaveformSrv(
00181     ethercat_trigger_controllers::SetMultiWaveform::Request &req,
00182     ethercat_trigger_controllers::SetMultiWaveform::Response &resp)
00183 {
00184   std::vector<boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Header> > > new_pubs;
00185   new_pubs.reserve(config_.transitions.size());
00186   ethercat_trigger_controllers::MultiWaveform &new_config = req.waveform;
00187 
00188   double prev_time = -1; // There is a check for negative values below.
00189   double now = ros::Time::now().toSec();
00190   double new_transition_period = round((now - new_config.zero_offset) / new_config.period);
00191   double current_period_start = new_config.zero_offset + new_transition_period * new_config.period;
00192   double now_offset = now - current_period_start;
00193   unsigned int new_transition_index = 0;
00194   resp.success = true;
00195 
00196   if (new_transition_period <= 0)
00197   {
00198     resp.status_message = "MultiTrigger period must be >0.";
00199     resp.success = false;
00200   }
00201 
00202   for (std::vector<ethercat_trigger_controllers::MultiWaveformTransition>::iterator trans = new_config.transitions.begin();
00203       trans != new_config.transitions.end() && resp.success; trans++)
00204   {
00205     if (trans->time < now_offset)
00206       new_transition_index++;
00207 
00208     if (trans->time < 0 || trans->time >= new_config.period)
00209     {
00210       resp.status_message = (boost::format("MultiTriggerController::setMultiWaveformSrv transition time (%f) must be >= 0 and < period (%f).")%
00211         trans->time%new_config.period).str();
00212       resp.success = false;
00213     }
00214     
00215     if (prev_time >= trans->time)
00216     {
00217       resp.status_message = (boost::format("MultiTriggerController::setMultiWaveformSrv transition times must be in increasing order. %f >= %f")% 
00218           prev_time%trans->time).str();
00219       resp.success = false;
00220     }
00221   }
00222 
00223   if (new_transition_index == new_config.transitions.size())
00224   {
00225     new_transition_index = 0;
00226     new_transition_period++;
00227   }
00228 
00229   double new_transition_time = current_period_start + new_config.transitions[new_transition_index].time;
00230   
00231 //  ROS_DEBUG("MultiTriggerController::setMultiWaveformSrv completed successfully"
00232 //      " rr=%f ph=%f al=%i r=%i p=%i dc=%f.", config_.rep_rate, config_.phase,
00233 //      config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
00234 
00235   if (resp.success)
00236   { 
00237     for (std::vector<ethercat_trigger_controllers::MultiWaveformTransition>::iterator trans = new_config.transitions.begin();
00238         trans != new_config.transitions.end() && resp.success; trans++)
00239     {
00240       boost::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Header> > new_pub;
00241         
00242       if (trans->topic.compare("-"))
00243         new_pub.reset(new realtime_tools::RealtimePublisher<std_msgs::Header>(node_handle_, trans->topic, 10));
00244 
00245       new_pubs.push_back(new_pub);
00246     }
00247 
00248     boost::mutex::scoped_lock lock(config_mutex_);
00249     config_ = new_config;
00250     pubs_ = new_pubs;
00251     transition_period_ = new_transition_period;
00252     transition_index_ = new_transition_index;
00253     transition_time_ = new_transition_time;
00254     waveform_.publish(req.waveform);
00255   }
00256   else
00257     ROS_ERROR("%s", resp.status_message.c_str());
00258   
00259   return true;
00260 }


ethercat_trigger_controllers
Author(s): Blaise Gassend
autogenerated on Thu Aug 27 2015 14:23:06