generic_hw_interface.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2015, University of Colorado, Boulder
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Univ of CO, Boulder nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Dave Coleman
   Desc:   Example ros_control hardware interface that performs a perfect control loop for
   simulation
*/

#ifndef GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H
#define GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H

// C++
#include <boost/scoped_ptr.hpp>

// ROS
#include <ros/ros.h>
#include <urdf/model.h>

// ROS Controls
#include <hardware_interface/robot_hw.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/joint_command_interface.h>
#include <controller_manager/controller_manager.h>
#include <joint_limits_interface/joint_limits.h>
#include <joint_limits_interface/joint_limits_interface.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <joint_limits_interface/joint_limits_urdf.h>

namespace ros_control_boilerplate
{
class GenericHWInterface : public hardware_interface::RobotHW
{
public:
  GenericHWInterface(const ros::NodeHandle& nh, urdf::Model* urdf_model = NULL);

  virtual ~GenericHWInterface()
  {
  }

  virtual void init();

  virtual void read(ros::Duration& elapsed_time) = 0;

  virtual void read(const ros::Time& /*time*/, const ros::Duration& period) override
  {
    ros::Duration elapsed_time = period;
    read(elapsed_time);
  }

  virtual void write(ros::Duration& elapsed_time) = 0;

  virtual void write(const ros::Time& /*time*/, const ros::Duration& period) override
  {
    ros::Duration elapsed_time = period;
    write(elapsed_time);
  }

  virtual void reset();

  virtual bool canSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                         const std::list<hardware_interface::ControllerInfo>& stop_list) const
  {
    return true;
  }

  virtual void doSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
                        const std::list<hardware_interface::ControllerInfo>& stop_list)
  {
  }

  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);

  virtual void enforceLimits(ros::Duration& period) = 0;

  virtual void printState();
  std::string printStateHelper();

  std::string printCommandHelper();

protected:
  virtual void loadURDF(const ros::NodeHandle& nh, std::string param_name);

  // Short name of this class
  std::string name_;

  // Startup and shutdown of the internal node inside a roscpp program
  ros::NodeHandle nh_;

  // Hardware interfaces
  hardware_interface::JointStateInterface joint_state_interface_;
  hardware_interface::PositionJointInterface position_joint_interface_;
  hardware_interface::VelocityJointInterface velocity_joint_interface_;
  hardware_interface::EffortJointInterface effort_joint_interface_;

  // Joint limits interfaces - Saturation
  joint_limits_interface::PositionJointSaturationInterface pos_jnt_sat_interface_;
  joint_limits_interface::VelocityJointSaturationInterface vel_jnt_sat_interface_;
  joint_limits_interface::EffortJointSaturationInterface eff_jnt_sat_interface_;

  // Joint limits interfaces - Soft limits
  joint_limits_interface::PositionJointSoftLimitsInterface pos_jnt_soft_limits_;
  joint_limits_interface::VelocityJointSoftLimitsInterface vel_jnt_soft_limits_;
  joint_limits_interface::EffortJointSoftLimitsInterface eff_jnt_soft_limits_;

  // Configuration
  std::vector<std::string> joint_names_;
  std::size_t num_joints_;
  urdf::Model* urdf_model_;

  // Modes
  bool use_rosparam_joint_limits_;
  bool use_soft_limits_if_available_;

  // States
  std::vector<double> joint_position_;
  std::vector<double> joint_velocity_;
  std::vector<double> joint_effort_;

  // Commands
  std::vector<double> joint_position_command_;
  std::vector<double> joint_velocity_command_;
  std::vector<double> joint_effort_command_;

  // Copy of limits, in case we need them later in our control stack
  std::vector<double> joint_position_lower_limits_;
  std::vector<double> joint_position_upper_limits_;
  std::vector<double> joint_velocity_limits_;
  std::vector<double> joint_effort_limits_;

};  // class

}  // namespace ros_control_boilerplate

#endif  // GENERIC_ROS_CONTROL_GENERIC_HW_INTERFACE_H