generic_hw_control_loop.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, University of Colorado, Boulder
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the Univ of CO, Boulder nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman
00036    Desc:   Example control loop for reading, updating, and writing commands to a hardware interface
00037    using MONOTOIC system time
00038 */
00039 
00040 #include <ros_control_boilerplate/generic_hw_control_loop.h>
00041 
00042 // ROS parameter loading
00043 #include <rosparam_shortcuts/rosparam_shortcuts.h>
00044 
00045 namespace ros_control_boilerplate
00046 {
00047 GenericHWControlLoop::GenericHWControlLoop(
00048     ros::NodeHandle& nh, boost::shared_ptr<ros_control_boilerplate::GenericHWInterface> hardware_interface)
00049   : nh_(nh), hardware_interface_(hardware_interface)
00050 {
00051   // Create the controller manager
00052   controller_manager_.reset(new controller_manager::ControllerManager(hardware_interface_.get(), nh_));
00053 
00054   // Load rosparams
00055   ros::NodeHandle rpsnh(nh, name_);
00056   std::size_t error = 0;
00057   error += !rosparam_shortcuts::get(name_, rpsnh, "loop_hz", loop_hz_);
00058   error += !rosparam_shortcuts::get(name_, rpsnh, "cycle_time_error_threshold", cycle_time_error_threshold_);
00059   rosparam_shortcuts::shutdownIfError(name_, error);
00060 
00061   // Get current time for use with first update
00062   clock_gettime(CLOCK_MONOTONIC, &last_time_);
00063 
00064   // Start timer
00065   ros::Duration desired_update_freq_ = ros::Duration(1 / loop_hz_);
00066   non_realtime_loop_ = nh_.createTimer(desired_update_freq_, &GenericHWControlLoop::update, this);
00067 }
00068 
00069 void GenericHWControlLoop::update(const ros::TimerEvent& e)
00070 {
00071   // Get change in time
00072   clock_gettime(CLOCK_MONOTONIC, &current_time_);
00073   elapsed_time_ =
00074       ros::Duration(current_time_.tv_sec - last_time_.tv_sec + (current_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
00075   last_time_ = current_time_;
00076   // ROS_DEBUG_STREAM_THROTTLE_NAMED(1, "generic_hw_main","Sampled update loop with elapsed
00077   // time " << elapsed_time_.toSec());
00078 
00079   // Error check cycle time
00080   const double cycle_time_error = (elapsed_time_ - desired_update_freq_).toSec();
00081   if (cycle_time_error > cycle_time_error_threshold_)
00082   {
00083     ROS_WARN_STREAM_NAMED(name_, "Cycle time exceeded error threshold by: "
00084                                      << cycle_time_error << ", cycle time: " << elapsed_time_
00085                                      << ", threshold: " << cycle_time_error_threshold_);
00086   }
00087 
00088   // Input
00089   hardware_interface_->read(elapsed_time_);
00090 
00091   // Control
00092   controller_manager_->update(ros::Time::now(), elapsed_time_);
00093 
00094   // Output
00095   hardware_interface_->write(elapsed_time_);
00096 }
00097 
00098 }  // namespace


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Thu Jun 6 2019 20:37:19