trigger_controller.cpp
Go to the documentation of this file.
1 
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2009, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the Willow Garage nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
61 #include "ros/console.h"
63 
65 
66 using std::string;
67 using namespace controller;
68 
70 {
71  ROS_DEBUG("creating controller...");
72 }
73 
74 TriggerController::~TriggerController()
75 {
76 }
77 
79 {
80  return getTick(robot_->getTime(), config_);
81 }
82 
84 {
85  ros::Time curtime = robot_->getTime();
86  double tick = getTick(curtime, config_);
87  bool active = false;
88 
89  if (config_.running)
90  {
91  if (config_.pulsed)
92  {
93  active = (floor(prev_tick_) != floor(tick));
94  //if (active)
95  // ROS_INFO("Triggered (%s)", actuator_name_.c_str()); // KILLME
96  }
97  else
98  {
99  active = fmod(tick, 1) < config_.duty_cycle;
100  //if (active != fmod(prev_tick_, 1) < config_.duty_cycle)
101  // ROS_INFO("Changed to: %i (%s)", active, actuator_name_.c_str()); // KILLME
102  }
103  }
104 
105  //if (actuator_command_->digital_out_ && !(active ^ config_.active_low))
106  // ROS_DEBUG("digital out falling at time %f", robot_->getTime());
107 
109  {
110  //ROS_WARN("Contention on digital output %s. Is %i, expected %i.", actuator_name_.c_str(), digital_out_command_->data_, last_out_);
111  }
112 
113  digital_out_command_->data_ = active ^ config_.active_low;
114 
116  {
117  if (falling_edge_pub_ && falling_edge_pub_->trylock())
118  {
119  falling_edge_pub_->msg_.stamp = curtime;
120  falling_edge_pub_->unlockAndPublish();
121  //ROS_DEBUG("Published rising edge from %s", actuator_name_.c_str());
122  }
123  else
124  {
125  //ROS_WARN("Unable to publish on falling edge of TriggerController %s.", actuator_name_.c_str());
126  }
127  }
128  else if (!last_out_ && digital_out_command_->data_)
129  {
130  if (rising_edge_pub_ && rising_edge_pub_->trylock())
131  {
132  rising_edge_pub_->msg_.stamp = curtime;
133  rising_edge_pub_->unlockAndPublish();
134  //ROS_DEBUG("Published falling edge from %s", actuator_name_.c_str());
135  }
136  else
137  {
138  //ROS_WARN("Unable to publish on rising edge of TriggerController %s.", actuator_name_.c_str());
139  }
140  }
141 
142  // ROS_INFO("digital out: %i (%s) %i", actuator_command_->digital_out_, actuator_name_.c_str(), last_out_);
143 
145 
146  prev_tick_ = tick;
147 }
148 
150 {
151  node_handle_ = n;
152 
153  ROS_DEBUG("LOADING TRIGGER CONTROLLER NODE");
154  //string prefix = config->Attribute("name");
155  //ROS_DEBUG_STREAM("the prefix is "<<prefix);
156 
157  assert(robot);
158  robot_=robot;
159 
160  ROS_DEBUG("TriggerController::init starting");
161 
162  // Get the actuator name.
163 
164  if (!n.getParam("actuator", actuator_name_)){
165  ROS_ERROR("TriggerController was not given an actuator.");
166  return false;
167  }
168 
170  if (!digital_out)
171  {
172  ROS_ERROR("TriggerController could not find digital out named \"%s\".",
173  actuator_name_.c_str());
174  return false;
175  }
176 
177  digital_out_command_ = &digital_out->command_;
178  digital_out_command_->data_ = false;
179  last_out_ = false;
180 
181  // Get the startup configuration (pulsed or constant)
182 
183 #define bparam(name, var, val) \
184  {\
185  bool tmp;\
186  n.param(name, tmp, val);\
187  var = tmp;\
188  }
189  n.param("rep_rate", config_.rep_rate, 1.);
190  n.param("phase", config_.phase, 0.);
191  n.param("duty_cycle", config_.duty_cycle, .5);
192  bparam("active_low", config_.active_low, false);
193  bparam("running", config_.running, false);
194  bparam("pulsed", config_.pulsed, true);
195 #undef bparam
196 
197  prev_tick_ = getTick();
198 
200 
201  rising_edge_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Header>(n, "rising_edge_timestamps", 10));
202  falling_edge_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Header>(n, "falling_edge_timestamps", 10));
203 
204  ROS_DEBUG("TriggerController::init completed successfully"
205  " rr=%f ph=%f al=%i r=%i p=%i dc=%f.",
206  config_.rep_rate, config_.phase, config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
207 
208  return true;
209 }
210 
213  ethercat_trigger_controllers::SetWaveform::Response &resp)
214 {
215  // FIXME This should be safe despite the asynchronous barrier. Should I
216  // be doing anything special to ensure that things get written in order?
217  config_.running = false; // Turn off pulsing before we start.
218  config_.rep_rate = req.rep_rate;
219  config_.phase = req.phase;
220  config_.duty_cycle = req.duty_cycle;
221  config_.active_low = !!req.active_low;
222  config_.pulsed = !!req.pulsed;
223  config_.running = !!req.running;
224 
225  ROS_DEBUG("TriggerController::setWaveformSrv completed successfully"
226  " rr=%f ph=%f al=%i r=%i p=%i dc=%f.", config_.rep_rate, config_.phase,
227  config_.active_low, config_.running, config_.pulsed, config_.duty_cycle);
228 
229  return true;
230 }
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Header > > falling_edge_pub_
pr2_hardware_interface::DigitalOutCommand * digital_out_command_
pr2_hardware_interface::HardwareInterface * hw_
ros::ServiceServer set_waveform_handle_
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
DigitalOut * getDigitalOut(const std::string &name) const
boost::scoped_ptr< realtime_tools::RealtimePublisher< std_msgs::Header > > rising_edge_pub_
pr2_mechanism_model::RobotState * robot_
PLUGINLIB_EXPORT_CLASS(pr2_mechanism_model::PR2BeltCompensatorTransmission, pr2_mechanism_model::Transmission) namespace pr2_mechanism_model
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n)
ethercat_trigger_controllers::SetWaveform::Request trigger_configuration
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR(...)
#define bparam(name, var, val)
bool setWaveformSrv(trigger_configuration &req, ethercat_trigger_controllers::SetWaveform::Response &resp)
#define ROS_DEBUG(...)


ethercat_trigger_controllers
Author(s): Blaise Gassend
autogenerated on Mon Jun 10 2019 14:26:16