Merge pull request #29 from davetcoleman/hydro-devel

Reviewed by @jbohren

Fixed PID destructor bug, cleaned up code

effort_controllers/CMakeLists.txt

@@ -24,11 +24,12 @@ else()
     src/joint_effort_controller.cpp include/effort_controllers/joint_effort_controller.h
     src/joint_velocity_controller.cpp include/effort_controllers/joint_velocity_controller.h
     src/joint_position_controller.cpp include/effort_controllers/joint_position_controller.h)
+
   target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
   # Declare catkin package
   catkin_package(
-    CATKIN_DEPENDS controller_interface controllers_msgs control_toolbox realtime_tools urdf forward_command_controller
+    CATKIN_DEPENDS controller_interface controllers_msgs control_toolbox realtime_tools urdf forward_command_controller dynamic_reconfigure
     INCLUDE_DIRS include
     LIBRARIES ${PROJECT_NAME}
     )

effort_controllers/include/effort_controllers/joint_position_controller.h

@@ -73,7 +73,6 @@
 #include <controllers_msgs/JointControllerState.h>
 #include <realtime_tools/realtime_buffer.h>
 
-
 namespace effort_controllers
 {
 
@@ -84,7 +83,19 @@ class JointPositionController: public controller_interface::Controller<hardware_
   JointPositionController();
   ~JointPositionController();
 
-  bool init(hardware_interface::EffortJointInterface*robot, const std::string &joint_name,const control_toolbox::Pid &pid);
+  /** \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 hw 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(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n);
 
   /*!
@@ -94,31 +105,52 @@ class JointPositionController: public controller_interface::Controller<hardware_
    */
   void setCommand(double cmd);
 
+  /** \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 Set the PID parameters
+   */
   void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+  /**
+   * \brief Get the PID parameters
+   */
   void setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min);
 
+  /**
+   * \brief Get the name of the joint this controller uses
+   */
   std::string getJointName();
+
   hardware_interface::JointHandle joint_;
   boost::shared_ptr<const urdf::Joint> joint_urdf_;
   realtime_tools::RealtimeBuffer<double> command_;             /**< Last commanded position. */
 
 private:
   int loop_count_;
-  control_toolbox::Pid pid_controller_;       /**< Internal PID controller. */
+  boost::scoped_ptr<control_toolbox::Pid> pid_controller_;       /**< Internal PID controller. */
 
   boost::scoped_ptr<
     realtime_tools::RealtimePublisher<
       controllers_msgs::JointControllerState> > controller_state_publisher_ ;
 
   ros::Subscriber sub_command_;
+
+  /**
+   * \brief Callback from /command subscriber for setpoint
+   */
   void setCommandCB(const std_msgs::Float64ConstPtr& msg);
+
 };
 
 } // namespace

effort_controllers/package.xml

@@ -19,12 +19,15 @@
   <build_depend>realtime_tools</build_depend> 
   <build_depend>urdf</build_depend> 
   <build_depend>forward_command_controller</build_depend>
-  <run_depend>forward_command_controller</run_depend>
+  <build_depend>dynamic_reconfigure</build_depend>
+
   <run_depend>controller_interface</run_depend> 
   <run_depend>controllers_msgs</run_depend> 
   <run_depend>control_toolbox</run_depend> 
   <run_depend>realtime_tools</run_depend> 
   <run_depend>urdf</run_depend> 
+  <run_depend>forward_command_controller</run_depend>
+  <run_depend>dynamic_reconfigure</run_depend>
 
   <export>
     <controller_interface plugin="${prefix}/effort_controllers_plugins.xml"/>

effort_controllers/src/joint_position_controller.cpp

@@ -33,71 +33,77 @@
  *  POSSIBILITY OF SUCH DAMAGE.
  *********************************************************************/
 
+/*
+ Author: Vijay Pradeep
+ Contributors: Jonathan Bohren, Wim Meeussen, Dave Coleman
+ Desc: Effort(force)-based position controller using basic PID loop
+*/
+
 #include <effort_controllers/joint_position_controller.h>
 #include <angles/angles.h>
 #include <pluginlib/class_list_macros.h>
 
 namespace effort_controllers {
 
 JointPositionController::JointPositionController()
-: loop_count_(0)
+  : loop_count_(0)
 {}
 
 JointPositionController::~JointPositionController()
 {
   sub_command_.shutdown();
 }
 
-bool JointPositionController::init(hardware_interface::EffortJointInterface *robot, 
-				   const std::string &joint_name, const control_toolbox::Pid &pid)
-{
-  joint_ = robot->getHandle(joint_name);
-  pid_controller_ = pid;
-
-  // get urdf info about joint
-  urdf::Model urdf;
-  if (!urdf.initParam("robot_description")){
-    ROS_ERROR("Failed to parse urdf file");
-    return false;
-  }
-  joint_urdf_ = urdf.getJoint(joint_name);
-  if (!joint_urdf_){
-    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
-    return false;
-  }
-
-  return true;
-}
-
 bool JointPositionController::init(hardware_interface::EffortJointInterface *robot, ros::NodeHandle &n)
 {
+  // Get joint name from parameter server
   std::string joint_name;
-  if (!n.getParam("joint", joint_name)) {
+  if (!n.getParam("joint", joint_name)) 
+  {
     ROS_ERROR("No joint given (namespace: %s)", n.getNamespace().c_str());
     return false;
   }
 
-  control_toolbox::Pid pid;
-  if (!pid.init(ros::NodeHandle(n, "pid")))
+  // Load PID Controller using gains set on parameter server
+  pid_controller_.reset(new control_toolbox::Pid());
+  if (!pid_controller_->init(ros::NodeHandle(n, "pid")))
     return false;
 
+  // Start realtime state publisher
   controller_state_publisher_.reset(
     new realtime_tools::RealtimePublisher<controllers_msgs::JointControllerState>(n, "state", 1));
 
+  // Start command subscriber
   sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);
 
-  return init(robot, joint_name, pid);
-}
+  // Get joint handle from hardware interface
+  joint_ = robot->getHandle(joint_name);
 
+  // Get URDF info about joint
+  urdf::Model urdf;
+  if (!urdf.initParam("robot_description"))
+  {
+    ROS_ERROR("Failed to parse urdf file");
+    return false;
+  }
+  joint_urdf_ = urdf.getJoint(joint_name);
+  if (!joint_urdf_)
+  {
+    ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
+    return false;
+  }
+
+  return true;
+}
 
 void JointPositionController::setGains(const double &p, const double &i, const double &d, const double &i_max, const double &i_min)
 {
-  pid_controller_.setGains(p,i,d,i_max,i_min);
+  pid_controller_->setGains(p,i,d,i_max,i_min);
 }
 
 void JointPositionController::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
 {
-  pid_controller_.getGains(p,i,d,i_max,i_min);
+  pid_controller_->getGains(p,i,d,i_max,i_min);
 }
 
 std::string JointPositionController::getJointName()
@@ -108,20 +114,18 @@ std::string JointPositionController::getJointName()
 // Set the joint position command
 void JointPositionController::setCommand(double cmd)
 {
-  // the writeFromNonRT can be used in RT, if you have the guarantee that 
+  // the writeFromNonRT can be used in RT, if you have the guarantee that
   //  * no non-rt thread is calling the same function (we're not subscribing to ros callbacks)
   //  * there is only one single rt thread
   command_.writeFromNonRT(cmd);
 }
 
-
-void JointPositionController::starting(const ros::Time& time) 
+void JointPositionController::starting(const ros::Time& time)
 {
   command_.initRT(joint_.getPosition());
-  pid_controller_.reset();
+  pid_controller_->reset();
 }
 
-
 void JointPositionController::update(const ros::Time& time, const ros::Duration& period)
 {
   double command = *(command_.readFromRT());
@@ -132,10 +136,10 @@ void JointPositionController::update(const ros::Time& time, const ros::Duration&
   if (joint_urdf_->type == urdf::Joint::REVOLUTE)
   {
     angles::shortest_angular_distance_with_limits(joint_.getPosition(),
-						  command, 
-						  joint_urdf_->limits->lower, 
-						  joint_urdf_->limits->upper,
-						  error);
+      command,
+      joint_urdf_->limits->lower,
+      joint_urdf_->limits->upper,
+      error);
   }
   else if (joint_urdf_->type == urdf::Joint::CONTINUOUS)
   {
@@ -150,8 +154,8 @@ void JointPositionController::update(const ros::Time& time, const ros::Duration&
   vel_error = 0.0 - joint_.getVelocity();
 
   // Set the PID error and compute the PID command with nonuniform
-  // time step size. This also allows the user to pass in a precomputed derivative error. 
-  double commanded_effort = pid_controller_.computeCommand(error, vel_error, period); 
+  // time step size. This also allows the user to pass in a precomputed derivative error.
+  double commanded_effort = pid_controller_->computeCommand(error, vel_error, period);
   joint_.setCommand(commanded_effort);
 
 
@@ -170,10 +174,10 @@ void JointPositionController::update(const ros::Time& time, const ros::Duration&
 
       double dummy;
       getGains(controller_state_publisher_->msg_.p,
-               controller_state_publisher_->msg_.i,
-               controller_state_publisher_->msg_.d,
-               controller_state_publisher_->msg_.i_clamp,
-               dummy);
+        controller_state_publisher_->msg_.i,
+        controller_state_publisher_->msg_.d,
+        controller_state_publisher_->msg_.i_clamp,
+        dummy);
       controller_state_publisher_->unlockAndPublish();
     }
   }