generic_hw_interface.h
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 ros_control hardware interface that performs a perfect control loop for
37  simulation
38 */
39 
40 #ifndef GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H
41 #define GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H
42 
43 // ROS
44 #include <ros/ros.h>
45 #include <urdf/model.h>
46 
47 // ROS Controls
56 
58 {
60 class GenericHWInterface : public hardware_interface::RobotHW
61 {
62 public:
68  GenericHWInterface(const ros::NodeHandle& nh, urdf::Model* urdf_model = NULL);
69 
71  virtual ~GenericHWInterface()
72  {
73  }
74 
76  virtual void init();
77 
79  virtual void read(ros::Duration& elapsed_time) = 0;
80 
88  virtual void read(const ros::Time& /*time*/, const ros::Duration& period) override
89  {
90  ros::Duration elapsed_time = period;
91  read(elapsed_time);
92  }
93 
95  virtual void write(ros::Duration& elapsed_time) = 0;
96 
104  virtual void write(const ros::Time& /*time*/, const ros::Duration& period) override
105  {
106  ros::Duration elapsed_time = period;
107  write(elapsed_time);
108  }
109 
111  virtual void reset();
112 
119  virtual bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
120  const std::list<hardware_interface::ControllerInfo>& stop_list) const
121  {
122  return true;
123  }
124 
130  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
131  const std::list<hardware_interface::ControllerInfo>& stop_list)
132  {
133  }
134 
141  virtual void registerJointLimits(const hardware_interface::JointHandle& joint_handle_position,
142  const hardware_interface::JointHandle& joint_handle_velocity,
143  const hardware_interface::JointHandle& joint_handle_effort, std::size_t joint_id);
144 
146  virtual void enforceLimits(ros::Duration& period) = 0;
147 
149  virtual void printState();
150  std::string printStateHelper();
151 
153  std::string printCommandHelper();
154 
155 protected:
157  virtual void loadURDF(const ros::NodeHandle& nh, std::string param_name);
158 
159  // Short name of this class
160  std::string name_;
161 
162  // Startup and shutdown of the internal node inside a roscpp program
164 
165  // Hardware interfaces
170 
171  // Joint limits interfaces - Saturation
175 
176  // Joint limits interfaces - Soft limits
180 
181  // Configuration
182  std::vector<std::string> joint_names_;
183  std::size_t num_joints_;
185 
186  // Modes
189 
190  // States
191  std::vector<double> joint_position_;
192  std::vector<double> joint_velocity_;
193  std::vector<double> joint_effort_;
194 
195  // Commands
196  std::vector<double> joint_position_command_;
197  std::vector<double> joint_velocity_command_;
198  std::vector<double> joint_effort_command_;
199 
200  // Copy of limits, in case we need them later in our control stack
201  std::vector<double> joint_position_lower_limits_;
202  std::vector<double> joint_position_upper_limits_;
203  std::vector<double> joint_velocity_limits_;
204  std::vector<double> joint_effort_limits_;
205 
206 }; // class
207 
208 } // namespace ros_control_boilerplate
209 
210 #endif // GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H
ros_control_boilerplate::GenericHWInterface::enforceLimits
virtual void enforceLimits(ros::Duration &period)=0
ros_control_boilerplate::GenericHWInterface::pos_jnt_sat_interface_
joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_
Definition: generic_hw_interface.h:236
ros_control_boilerplate::GenericHWInterface::joint_position_command_
std::vector< double > joint_position_command_
Definition: generic_hw_interface.h:260
ros_control_boilerplate::GenericHWInterface::doSwitch
virtual void doSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list)
Perform (in non-realtime) all necessary hardware interface switches in order to start and stop the gi...
Definition: generic_hw_interface.h:194
ros_control_boilerplate::GenericHWInterface::nh_
ros::NodeHandle nh_
Definition: generic_hw_interface.h:227
ros_control_boilerplate::GenericHWInterface::read
virtual void read(ros::Duration &elapsed_time)=0
Read the state from the robot hardware.
ros_control_boilerplate
Definition: generic_hw_control_loop.h:44
hardware_interface::JointStateInterface
ros_control_boilerplate::GenericHWInterface::joint_position_lower_limits_
std::vector< double > joint_position_lower_limits_
Definition: generic_hw_interface.h:265
ros.h
ros_control_boilerplate::GenericHWInterface::joint_state_interface_
hardware_interface::JointStateInterface joint_state_interface_
Definition: generic_hw_interface.h:230
ros_control_boilerplate::GenericHWInterface::joint_effort_limits_
std::vector< double > joint_effort_limits_
Definition: generic_hw_interface.h:268
joint_limits_interface.h
ros_control_boilerplate::GenericHWInterface::use_soft_limits_if_available_
bool use_soft_limits_if_available_
Definition: generic_hw_interface.h:252
joint_limits_interface::VelocityJointSaturationInterface
ros_control_boilerplate::GenericHWInterface::use_rosparam_joint_limits_
bool use_rosparam_joint_limits_
Definition: generic_hw_interface.h:251
joint_limits_interface::VelocityJointSoftLimitsInterface
ros_control_boilerplate::GenericHWInterface::eff_jnt_sat_interface_
joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_
Definition: generic_hw_interface.h:238
urdf::Model
joint_limits.h
ros_control_boilerplate::GenericHWInterface::init
virtual void init()
Initialize the hardware interface.
Definition: generic_hw_interface.cpp:96
ros_control_boilerplate::GenericHWInterface::joint_velocity_command_
std::vector< double > joint_velocity_command_
Definition: generic_hw_interface.h:261
ros_control_boilerplate::GenericHWInterface::joint_velocity_
std::vector< double > joint_velocity_
Definition: generic_hw_interface.h:256
joint_limits_interface::PositionJointSaturationInterface
controller_manager.h
ros_control_boilerplate::GenericHWInterface::~GenericHWInterface
virtual ~GenericHWInterface()
Destructor.
Definition: generic_hw_interface.h:135
hardware_interface::VelocityJointInterface
joint_limits_interface::EffortJointSoftLimitsInterface
ros_control_boilerplate::GenericHWInterface::vel_jnt_sat_interface_
joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_
Definition: generic_hw_interface.h:237
joint_command_interface.h
hardware_interface::RobotHW
ros_control_boilerplate::GenericHWInterface::joint_effort_
std::vector< double > joint_effort_
Definition: generic_hw_interface.h:257
ros_control_boilerplate::GenericHWInterface::joint_names_
std::vector< std::string > joint_names_
Definition: generic_hw_interface.h:246
ros_control_boilerplate::GenericHWInterface::registerJointLimits
virtual void registerJointLimits(const hardware_interface::JointHandle &joint_handle_position, const hardware_interface::JointHandle &joint_handle_velocity, const hardware_interface::JointHandle &joint_handle_effort, std::size_t joint_id)
Register the limits of the joint specified by joint_id and joint_handle. The limits are retrieved fro...
Definition: generic_hw_interface.cpp:151
ros_control_boilerplate::GenericHWInterface::eff_jnt_soft_limits_
joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_limits_
Definition: generic_hw_interface.h:243
model.h
ros_control_boilerplate::GenericHWInterface::printCommandHelper
std::string printCommandHelper()
Helper for debugging a joint's command.
Definition: generic_hw_interface.cpp:318
ros_control_boilerplate::GenericHWInterface::joint_position_upper_limits_
std::vector< double > joint_position_upper_limits_
Definition: generic_hw_interface.h:266
hardware_interface::EffortJointInterface
joint_limits_rosparam.h
ros_control_boilerplate::GenericHWInterface::name_
std::string name_
Definition: generic_hw_interface.h:224
ros_control_boilerplate::GenericHWInterface::printState
virtual void printState()
Helper for debugging a joint's state.
Definition: generic_hw_interface.cpp:297
ros_control_boilerplate::GenericHWInterface::velocity_joint_interface_
hardware_interface::VelocityJointInterface velocity_joint_interface_
Definition: generic_hw_interface.h:232
ros_control_boilerplate::GenericHWInterface::joint_effort_command_
std::vector< double > joint_effort_command_
Definition: generic_hw_interface.h:262
ros_control_boilerplate::GenericHWInterface::pos_jnt_soft_limits_
joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_limits_
Definition: generic_hw_interface.h:241
ros_control_boilerplate::GenericHWInterface::reset
virtual void reset()
Set all members to default values.
Definition: generic_hw_interface.cpp:290
ros_control_boilerplate::GenericHWInterface::write
virtual void write(ros::Duration &elapsed_time)=0
Write the command to the robot hardware.
ros_control_boilerplate::GenericHWInterface::loadURDF
virtual void loadURDF(const ros::NodeHandle &nh, std::string param_name)
Get the URDF XML from the parameter server.
Definition: generic_hw_interface.cpp:332
joint_state_interface.h
hardware_interface::JointHandle
ros::Time
joint_limits_urdf.h
ros_control_boilerplate::GenericHWInterface::GenericHWInterface
GenericHWInterface(const ros::NodeHandle &nh, urdf::Model *urdf_model=NULL)
Constructor.
Definition: generic_hw_interface.cpp:79
joint_limits_interface::PositionJointSoftLimitsInterface
ros_control_boilerplate::GenericHWInterface::vel_jnt_soft_limits_
joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_limits_
Definition: generic_hw_interface.h:242
ros_control_boilerplate::GenericHWInterface::joint_position_
std::vector< double > joint_position_
Definition: generic_hw_interface.h:255
ros_control_boilerplate::GenericHWInterface::printStateHelper
std::string printStateHelper()
Definition: generic_hw_interface.cpp:304
ros_control_boilerplate::GenericHWInterface::num_joints_
std::size_t num_joints_
Definition: generic_hw_interface.h:247
ros_control_boilerplate::GenericHWInterface::urdf_model_
urdf::Model * urdf_model_
Definition: generic_hw_interface.h:248
robot_hw.h
ros_control_boilerplate::GenericHWInterface::canSwitch
virtual bool canSwitch(const std::list< hardware_interface::ControllerInfo > &start_list, const std::list< hardware_interface::ControllerInfo > &stop_list) const
Check (in non-realtime) if given controllers could be started and stopped from the current state of t...
Definition: generic_hw_interface.h:183
ros_control_boilerplate::GenericHWInterface::joint_velocity_limits_
std::vector< double > joint_velocity_limits_
Definition: generic_hw_interface.h:267
hardware_interface::PositionJointInterface
ros::Duration
ros_control_boilerplate::GenericHWInterface::effort_joint_interface_
hardware_interface::EffortJointInterface effort_joint_interface_
Definition: generic_hw_interface.h:233
ros::NodeHandle
joint_limits_interface::EffortJointSaturationInterface
ros_control_boilerplate::GenericHWInterface::position_joint_interface_
hardware_interface::PositionJointInterface position_joint_interface_
Definition: generic_hw_interface.h:231


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Wed Mar 2 2022 00:52:14