generic_hw_control_loop.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, University of Colorado, Boulder
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Univ of CO, Boulder nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman
36  Desc: Example control loop for reading, updating, and writing commands to a hardware interface
37  using MONOTOIC system time
38 */
39 
41 
42 // ROS parameter loading
44 
46 {
49  : nh_(nh), hardware_interface_(hardware_interface)
50 {
51  // Create the controller manager
53 
54  // Load rosparams
55  ros::NodeHandle rpsnh(nh, name_);
56  std::size_t error = 0;
57  error += !rosparam_shortcuts::get(name_, rpsnh, "loop_hz", loop_hz_);
58  error += !rosparam_shortcuts::get(name_, rpsnh, "cycle_time_error_threshold", cycle_time_error_threshold_);
60 
61  // Get current time for use with first update
62  clock_gettime(CLOCK_MONOTONIC, &last_time_);
63 
65 }
66 
68 {
69  ros::Rate rate(loop_hz_);
70  while(ros::ok()) {
71  update();
72  rate.sleep();
73  }
74 }
75 
77 {
78  // Get change in time
79  clock_gettime(CLOCK_MONOTONIC, &current_time_);
81  ros::Duration(current_time_.tv_sec - last_time_.tv_sec + (current_time_.tv_nsec - last_time_.tv_nsec) / BILLION);
83  ros::Time now = ros::Time::now();
84  // ROS_DEBUG_STREAM_THROTTLE_NAMED(1, "generic_hw_main","Sampled update loop with elapsed
85  // time " << elapsed_time_.toSec());
86 
87  // Error check cycle time
88  const double cycle_time_error = (elapsed_time_ - desired_update_period_).toSec();
89  if (cycle_time_error > cycle_time_error_threshold_)
90  {
91  ROS_WARN_STREAM_NAMED(name_, "Cycle time exceeded error threshold by: "
92  << cycle_time_error << ", cycle time: " << elapsed_time_
93  << ", threshold: " << cycle_time_error_threshold_);
94  }
95 
96  // Input
98 
99  // Control
100  controller_manager_->update(now, elapsed_time_);
101 
102  // Output
103  hardware_interface_->write(now, elapsed_time_);
104 }
105 
106 } // namespace
GenericHWControlLoop(ros::NodeHandle &nh, boost::shared_ptr< hardware_interface::RobotHW > hardware_interface)
Constructor.
boost::shared_ptr< hardware_interface::RobotHW > hardware_interface_
Abstract Hardware Interface for your robot.
ROSCPP_DECL bool ok()
boost::shared_ptr< controller_manager::ControllerManager > controller_manager_
ROS Controller Manager and Runner.
bool sleep()
void shutdownIfError(const std::string &parent_name, std::size_t error_count)
static Time now()
bool get(const std::string &parent_name, const ros::NodeHandle &nh, const std::string &param_name, bool &value)
#define ROS_WARN_STREAM_NAMED(name, args)


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Thu Feb 25 2021 03:58:54