ethercat_trigger_controllers: trigger_controller.cpp Source 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 
00060 #include "ethercat_trigger_controllers/trigger_controller.h"
00061 #include "ros/console.h"
00062 #include "pluginlib/class_list_macros.h"
00063 
00064 PLUGINLIB_DECLARE_CLASS(ethercat_trigger_controllers, TriggerController, controller::TriggerController, pr2_controller_interface::Controller)
00065 
00066 using std::string;
00067 using namespace controller;
00068 
00069 TriggerController::TriggerController()
00070 {
00071   ROS_DEBUG("creating controller...");
00072 }
00073 
00074 TriggerController::~TriggerController()
00075 {
00076 }
00077 
00078 double TriggerController::getTick()
00079 {
00080   return getTick(robot_->getTime(), config_);
00081 }
00082 
00083 void TriggerController::update()
00084 {
00085   ros::Time curtime = robot_->getTime();
00086   double tick = getTick(curtime, config_);
00087   bool active = false;
00088 
00089   if (config_.running)
00090   {
00091     if (config_.pulsed)
00092     {
00093       active = (floor(prev_tick_) != floor(tick));
00094       //if (active)
00095       //  ROS_INFO("Triggered (%s)", actuator_name_.c_str()); // KILLME
00096     }
00097     else
00098     {
00099       active = fmod(tick, 1) < config_.duty_cycle;
00100       //if (active != fmod(prev_tick_, 1) < config_.duty_cycle)
00101       //  ROS_INFO("Changed to: %i (%s)", active, actuator_name_.c_str()); // KILLME
00102     }
00103   }
00104 
00105   //if (actuator_command_->digital_out_ && !(active ^ config_.active_low))
00106   //    ROS_DEBUG("digital out falling at time %f", robot_->getTime());
00107 
00108   if (digital_out_command_->data_ != last_out_)
00109   {
00110     //ROS_WARN("Contention on digital output %s. Is %i, expected %i.", actuator_name_.c_str(), digital_out_command_->data_, last_out_);
00111   }
00112 
00113   digital_out_command_->data_ = active ^ config_.active_low;
00114 
00115   if (last_out_ && !digital_out_command_->data_)
00116   {
00117     if (falling_edge_pub_ && falling_edge_pub_->trylock())
00118     {
00119       falling_edge_pub_->msg_.stamp = curtime;
00120       falling_edge_pub_->unlockAndPublish();
00121       //ROS_DEBUG("Published rising edge from %s", actuator_name_.c_str());
00122     }
00123     else
00124     {
00125       //ROS_WARN("Unable to publish on falling edge of TriggerController %s.", actuator_name_.c_str());
00126     }
00127   }
00128   else if (!last_out_ && digital_out_command_->data_)
00129   {
00130     if (rising_edge_pub_ && rising_edge_pub_->trylock())
00131     {
00132       rising_edge_pub_->msg_.stamp = curtime;
00133       rising_edge_pub_->unlockAndPublish();
00134       //ROS_DEBUG("Published falling edge from %s", actuator_name_.c_str());
00135     }
00136     else
00137     {
00138       //ROS_WARN("Unable to publish on rising edge of TriggerController %s.", actuator_name_.c_str());
00139     }
00140   }
00141   
00142   //  ROS_INFO("digital out: %i (%s) %i", actuator_command_->digital_out_, actuator_name_.c_str(), last_out_);
00143 
00144   last_out_ = digital_out_command_->data_;
00145 
00146   prev_tick_ = tick;
00147 }
00148 
00149 bool TriggerController::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle& n)
00150 {
00151   node_handle_ = n;
00152 
00153   ROS_DEBUG("LOADING TRIGGER CONTROLLER NODE");
00154   //string prefix = config->Attribute("name");
00155   //ROS_DEBUG_STREAM("the prefix is "<<prefix);
00156 
00157   assert(robot);
00158   robot_=robot;
00159 
00160   ROS_DEBUG("TriggerController::init starting");
00161 
00162   // Get the actuator name.
00163 
00164   if (!n.getParam("actuator", actuator_name_)){
00165     ROS_ERROR("TriggerController was not given an actuator.");
00166     return false;
00167   }
00168 
00169   pr2_hardware_interface::DigitalOut *digital_out = robot_->model_->hw_->getDigitalOut(actuator_name_);
00170   if (!digital_out)
00171   {
00172     ROS_ERROR("TriggerController could not find digital out named \"%s\".",
00173         actuator_name_.c_str());
00174     return false;
00175   }
00176 
00177   digital_out_command_ = &digital_out->command_;
00178   digital_out_command_->data_ = false;
00179   last_out_ = false;
00180 
00181   // Get the startup configuration (pulsed or constant)
00182 
00183 #define bparam(name, var, val) \
00184   {\
00185     bool tmp;\
00186     n.param(name, tmp, val);\
00187     var = tmp;\
00188   }
00189   n.param("rep_rate", config_.rep_rate, 1.);
00190   n.param("phase", config_.phase, 0.);
00191   n.param("duty_cycle", config_.duty_cycle, .5);
00192   bparam("active_low", config_.active_low, false);
00193   bparam("running", config_.running, false);
00194   bparam("pulsed", config_.pulsed, true);
00195 #undef bparam
00196 
00197   prev_tick_ = getTick();
00198 
00199   set_waveform_handle_ = node_handle_.advertiseService("set_waveform", &TriggerController::setWaveformSrv, this);
00200 
00201   rising_edge_pub_.reset(new realtime_tools::RealtimePublisher<roslib::Header>(n, "rising_edge_timestamps", 10));
00202   falling_edge_pub_.reset(new realtime_tools::RealtimePublisher<roslib::Header>(n, "falling_edge_timestamps", 10));
00203 
00204   ROS_DEBUG("TriggerController::init completed successfully"
00205       " rr=%f ph=%f al=%i r=%i p=%i dc=%f.",
00206       config_.rep_rate, config_.phase, config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
00207 
00208   return true;
00209 }
00210 
00211 bool TriggerController::setWaveformSrv(
00212     trigger_configuration &req,
00213     ethercat_trigger_controllers::SetWaveform::Response &resp)
00214 {
00215   // FIXME This should be safe despite the asynchronous barrier. Should I
00216   // be doing anything special to ensure that things get written in order?
00217   config_.running = false; // Turn off pulsing before we start.
00218   config_.rep_rate = req.rep_rate;
00219   config_.phase = req.phase;
00220   config_.duty_cycle = req.duty_cycle;
00221   config_.active_low = !!req.active_low;
00222   config_.pulsed = !!req.pulsed;
00223   config_.running = !!req.running;
00224 
00225   ROS_DEBUG("TriggerController::setWaveformSrv completed successfully"
00226       " rr=%f ph=%f al=%i r=%i p=%i dc=%f.", config_.rep_rate, config_.phase,
00227       config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
00228 
00229   return true;
00230 }