Add copies of upstream effort_controllers to take SharedHWInterface

The normal ros_controllers/effort_controllers used by the SawyerControllers to control each joint do not allow a subclass of the EffortJointInterface to be passed in as a replacement. Thus, we need to (temporarily) copy in the upstream effort_controllers so that we can change them to take the SharedJointInterface. We should remove these copies once we can upstream a change to make the init() and constructor virtual (or some similar modification) so that interfaces that inherit from EffortJointInterface can be used instead.
RethinkRobotics · Aug 3, 2017 · 430644e · 430644e
1 parent 32b9555
commit 430644e
Show file tree

Hide file tree

Showing 16 changed files with 1,020 additions and 13 deletions.
diff --git a/sawyer_sim_controllers/CMakeLists.txt b/sawyer_sim_controllers/CMakeLists.txt
@@ -30,6 +30,11 @@ add_library(${PROJECT_NAME}
   src/sawyer_velocity_controller.cpp
   src/sawyer_effort_controller.cpp
   src/sawyer_gravity_controller.cpp
+  ## Copies of upstream ros_controllers/effort_controllers:
+  #  (remove once inheritable change is upstreamed)
+  src/sawyer_joint_position_controller.cpp
+  src/sawyer_joint_velocity_controller.cpp
+  src/sawyer_joint_effort_controller.cpp
 )
 
 target_link_libraries(${PROJECT_NAME}

diff --git a/sawyer_sim_controllers/config/sawyer_sim_controllers_plugins.xml b/sawyer_sim_controllers/config/sawyer_sim_controllers_plugins.xml
@@ -32,4 +32,31 @@
         </description>
     </class>
 
+
+
+    <!-- Following 3 "sawyer_effort_controllers" are copies of upstream 'ros_controllers/effort_controllers' which should be removed once inheritence enabling change is upstreamed. -->
+    <class name="sawyer_sim_controllers/JointPositionController"
+           type="sawyer_effort_controllers::JointPositionController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointPositionController tracks position commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="sawyer_sim_controllers/JointVelocityController"
+           type="sawyer_effort_controllers::JointVelocityController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointVelocityController tracks velocity commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
+    <class name="sawyer_sim_controllers/JointEffortController"
+           type="sawyer_effort_controllers::JointEffortController"
+           base_class_type="controller_interface::ControllerBase">
+        <description>
+            The JointEffortController tracks effort commands. It expects a SharedJointInterface (EffortJointInterface) type of hardware interface.
+        </description>
+    </class>
+
 </library>
diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_effort_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_effort_controller.h
@@ -19,14 +19,14 @@
 
 #include <sawyer_sim_controllers/joint_array_controller.h>
 #include <intera_core_msgs/JointCommand.h>
-#include <effort_controllers/joint_effort_controller.h>
+#include <sawyer_sim_controllers/sawyer_joint_effort_controller.h>
 #include <ros/node_handle.h>
 
 #include <control_toolbox/pid.h>
 
 namespace sawyer_sim_controllers
 {
-  class SawyerEffortController : public sawyer_sim_controllers::JointArrayController<effort_controllers::JointEffortController>
+  class SawyerEffortController : public sawyer_sim_controllers::JointArrayController<sawyer_effort_controllers::JointEffortController>
   {
   public:
     virtual ~SawyerEffortController() {sub_joint_command_.shutdown();}

diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_gravity_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_gravity_controller.h
@@ -19,17 +19,17 @@
 
 #include <sawyer_sim_controllers/joint_array_controller.h>
 #include <intera_core_msgs/JointCommand.h>
-#include <effort_controllers/joint_effort_controller.h>
+#include <sawyer_sim_controllers/sawyer_joint_effort_controller.h>
 #include <ros/node_handle.h>
 
 #include <control_toolbox/pid.h>
 
 namespace sawyer_sim_controllers
 {
-  class SawyerGravityController : public sawyer_sim_controllers::JointArrayController<effort_controllers::JointEffortController>
+  class SawyerGravityController : public sawyer_sim_controllers::JointArrayController<sawyer_effort_controllers::JointEffortController>
   {
   public:
-    SawyerGravityController() : sawyer_sim_controllers::JointArrayController<effort_controllers::JointEffortController>() { };
+    SawyerGravityController() : sawyer_sim_controllers::JointArrayController<sawyer_effort_controllers::JointEffortController>() { };
 
     virtual ~SawyerGravityController() {sub_joint_command_.shutdown();}
     virtual bool init(sawyer_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n) override;

diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_joint_effort_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_joint_effort_controller.h
@@ -0,0 +1,69 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  Copyright (c) 2013, PAL Robotics, S.L.
+ *  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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef SAWYER_EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H
+#define SAWYER_EFFORT_CONTROLLERS__JOINT_EFFORT_CONTROLLER_H
+
+#include <forward_command_controller/forward_command_controller.h>
+
+#include <sawyer_hardware_interface/shared_joint_interface.h>
+
+namespace sawyer_effort_controllers
+{
+
+/**
+ * \brief Joint Effort Controller (torque or force)
+ *
+ * This class passes the commanded effort down to the joint.
+ *
+ * \section ROS interface
+ *
+ * \param type Must be "JointEffortController".
+ * \param joint Name of the joint to control.
+ *
+ * Subscribes to:
+ * - \b command (std_msgs::Float64) : The joint effort to apply.
+ */
+class JointEffortController : public forward_command_controller::ForwardCommandController<sawyer_hardware_interface::SharedJointInterface>
+{
+public:
+  bool init(sawyer_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n);
+  bool init(sawyer_hardware_interface::SharedJointInterface* hw, ros::NodeHandle &n, const std::string& ctrl_type);
+};
+
+}  // namespace
+
+#endif
diff --git a/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_joint_position_controller.h b/sawyer_sim_controllers/include/sawyer_sim_controllers/sawyer_joint_position_controller.h
@@ -0,0 +1,208 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008, Willow Garage, Inc.
+ *  Copyright (c) 2012, hiDOF, Inc.
+ *  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 Willow Garage 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.
+ *********************************************************************/
+
+#ifndef SAWYER_EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+#define SAWYER_EFFORT_CONTROLLERS__JOINT_POSITION_CONTROLLER_H
+
+/**
+   @class effort_controllers::JointPositionController
+   @brief Joint Position Controller
+
+   This class controls positon using a pid loop.
+
+   @section ROS ROS interface
+
+   @param type Must be "effort_controllers::JointPositionController"
+   @param joint Name of the joint to control.
+   @param pid Contains the gains for the PID loop around position.  See: control_toolbox::Pid
+
+   Subscribes to:
+
+   - @b command (std_msgs::Float64) : The joint position to achieve.
+
+   Publishes:
+
+   - @b state (control_msgs::JointControllerState) :
+     Current state of the controller, including pid error and gains.
+
+*/
+
+#include <ros/node_handle.h>
+#include <urdf/model.h>
+#include <control_toolbox/pid.h>
+#include <boost/scoped_ptr.hpp>
+#include <boost/thread/condition.hpp>
+#include <realtime_tools/realtime_publisher.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <controller_interface/controller.h>
+#include <control_msgs/JointControllerState.h>
+#include <std_msgs/Float64.h>
+#include <control_msgs/JointControllerState.h>
+#include <realtime_tools/realtime_buffer.h>
+
+#include <sawyer_hardware_interface/shared_joint_interface.h>
+
+
+namespace sawyer_effort_controllers
+{
+
+class JointPositionController: public controller_interface::Controller<sawyer_hardware_interface::SharedJointInterface>
+{
+public:
+
+  /**
+   * \brief Store position and velocity command in struct to allow easier realtime buffer usage
+   */
+  struct Commands
+  {
+    double position_; // Last commanded position
+    double velocity_; // Last commanded velocity
+    bool has_velocity_; // false if no velocity command has been specified
+  };
+
+  JointPositionController();
+  ~JointPositionController();
+
+  /** \brief The init function is called to initialize the controller from a
+   * non-realtime thread with a pointer to the hardware interface, itself,
+   * instead of a pointer to a RobotHW.
+   *
+   * \param robot The specific hardware interface used by this controller.
+   *
+   * \param n A NodeHandle in the namespace from which the controller
+   * should read its configuration, and where it should set up its ROS
+   * interface.
+   *
+   * \returns True if initialization was successful and the controller
+   * is ready to be started.
+   */
+  bool init(sawyer_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n);
+  bool init(sawyer_hardware_interface::SharedJointInterface *robot, ros::NodeHandle &n, const std::string& ctrl_type)
+  {
+    // ctrl_type is no-op for non-effort control
+    return init(robot, n);
+  }
+
+  /*!
+   * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+   *
+   * \param command
+   */
+  void setCommand(double pos_target);
+
+  /*!
+   * \brief Give set position of the joint for next update: revolute (angle) and prismatic (position)
+   *        Also supports a target velocity
+   *
+   * \param pos_target - position setpoint
+   * \param vel_target - velocity setpoint
+   */
+  void setCommand(double pos_target, double vel_target);
+
+  /** \brief This is called from within the realtime thread just before the
+   * first call to \ref update
+   *
+   * \param time The current time
+   */
+  void starting(const ros::Time& time);
+
+  /*!
+   * \brief Issues commands to the joint. Should be called at regular intervals
+   */
+  void update(const ros::Time& time, const ros::Duration& period);
+
+  /**
+   * \brief Get the PID parameters
+   */
+  void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+  /**
+   * \brief Get the PID parameters
+   */
+  void getGains(double &p, double &i, double &d, double &i_max, double &i_min, bool &antiwindup);
+
+  /**
+   * \brief Print debug info to console
+   */
+  void printDebug();
+
+  /**
+   * \brief Get the PID parameters
+   */
+  void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min, const bool &antiwindup = false);
+
+  /**
+   * \brief Get the name of the joint this controller uses
+   */
+  std::string getJointName();
+
+  /**
+   * \brief Get the current position of the joint
+   * \return current position
+   */
+  double getPosition();
+
+  hardware_interface::JointHandle joint_;
+  urdf::JointConstSharedPtr joint_urdf_;
+  realtime_tools::RealtimeBuffer<Commands> command_;
+  Commands command_struct_; // pre-allocated memory that is re-used to set the realtime buffer
+
+private:
+  int loop_count_;
+  control_toolbox::Pid pid_controller_;       /**< Internal PID controller. */
+
+  boost::scoped_ptr<
+    realtime_tools::RealtimePublisher<
+      control_msgs::JointControllerState> > controller_state_publisher_ ;
+
+  ros::Subscriber sub_command_;
+
+  /**
+   * \brief Callback from /command subscriber for setpoint
+   */
+  void setCommandCB(const std_msgs::Float64ConstPtr& msg);
+
+  /**
+   * \brief Check that the command is within the hard limits of the joint. Checks for joint
+   *        type first. Sets command to limit if out of bounds.
+   * \param command - the input to test
+   */
+  void enforceJointLimits(double &command);
+
+};
+
+} // namespace
+
+#endif