Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added dynamic reconfigure for PID gains #1

Merged
merged 13 commits into from
Jul 29, 2013
Merged

Added dynamic reconfigure for PID gains #1

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
d5f9923
Added dynamic reconfigure for PID gains
davetcoleman Jul 11, 2013
e2020db
Merged hydro-devel
davetcoleman Jul 11, 2013
1ec720c
Updated CMakeLists.txt and made fixes per Adolfo
davetcoleman Jul 15, 2013
3ed065a
Fixes per Austin review
davetcoleman Jul 15, 2013
8f83f74
Added realtime buffer to PID, re-ordered functions to more logical or…
davetcoleman Jul 18, 2013
712f2aa
Added realtime_tools as a dependency in package.xml and CMakeLists
davetcoleman Jul 18, 2013
04f11ae
Added new function getGainsConst that allows one to get the PID gains…
davetcoleman Jul 18, 2013
90ab218
Removed const read, added copy constructor and print values function
davetcoleman Jul 19, 2013
88d6bc8
Added test for getting/settings gains, copying/assigning pid class
davetcoleman Jul 22, 2013
c17668d
Made realtime_buffer copiable
davetcoleman Jul 22, 2013
d30b816
Compatibility changes for realtime_tools, tweaked getests
davetcoleman Jul 22, 2013
76761cd
Small fixes
davetcoleman Jul 23, 2013
17e41b5
Removed const getGains function
davetcoleman Jul 26, 2013
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
27 changes: 20 additions & 7 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,18 +19,30 @@ if(USE_ROSBUILD)
# rosbuild_add_executable(test_linear test/linear.cpp)

# Tests
rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp)
rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp)
target_link_libraries(test/pid_tests control_toolbox)

else()

# Load catkin and all dependencies required for this package
find_package(catkin REQUIRED COMPONENTS rosconsole message_generation tf roscpp angles)
find_package(catkin REQUIRED COMPONENTS
rosconsole
message_generation
tf
roscpp
angles
dynamic_reconfigure
)

include_directories(
include
include
${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})

# Dynamics reconfigure
generate_dynamic_reconfigure_options(
cfg/Parameters.cfg
)

# Add services and generate them
add_service_files(
FILES
Expand All @@ -49,16 +61,17 @@ else()
src/sinusoid.cpp
src/limited_proxy.cpp
)
add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg)

target_link_libraries(control_toolbox tinyxml ${catkin_LIBRARIES})
add_dependencies(control_toolbox ${PROJECT_NAME}_gencpp)

# Declare catkin package
catkin_package(
DEPENDS tinyxml
CATKIN_DEPENDS rosconsole tf roscpp angles
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
CATKIN_DEPENDS rosconsole tf roscpp angles dynamic_reconfigure
INCLUDE_DIRS include
LIBRARIES ${PROJECT_NAME}
)

# Tests
Expand All @@ -78,6 +91,6 @@ else()
)

install(DIRECTORY scripts/
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

endif()
54 changes: 54 additions & 0 deletions cfg/Parameters.cfg
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,54 @@
#! /usr/bin/env python
#*********************************************************************
#* Software License Agreement (BSD License)
#*
#* Copyright (c) 2013, Open Source Robotics Foundation
#* 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 Open Source Robotics Foundation
#* 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: Allows PID parameters, etc to be tuned in realtime using dynamic reconfigure


PACKAGE='control_toolbox'

from dynamic_reconfigure.parameter_generator_catkin import *

gen = ParameterGenerator()

# Name Type Level Description Default Min Max
gen.add( "p_gain" , double_t, 1,"Proportional gain.", 10.0 , 0 , 1000)
gen.add( "i_gain" , double_t, 1,"Integral gain.", 10.0 , 0 , 100)
gen.add( "d_gain" , double_t, 1,"Derivative gain.", 10.0 , 0 , 100)
gen.add( "i_clamp_min" , double_t, 1,"Min bounds for the integral windup", 10.0 , 0 , 100)
gen.add( "i_clamp_max" , double_t, 1,"Max bounds for the integral windup", 10.0 , 0 , 100)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe dynamic_reconfigure interacts with the parameter server; it may be useful to give these the same names and defaults and their matching parameters.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you mean the full path of the parameters on the parameter server, the Pid class doesn't know those before hand (it is passed a nodehandle at initialization).

If you mean we should name the gains "p", "i", and "d" - I tried that and wasted a lot of time debugging why it wasn't working. There is a bug that disallows you to name a dynamic reconfigure parameter "i" ros/dynamic_reconfigure#6

I documented the issue on the tutorial in the red box.

# PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py
exit(gen.generate(PACKAGE, "control_toolbox", "Parameters"))
26 changes: 25 additions & 1 deletion include/control_toolbox/pid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,11 @@


#include <string>
#include "ros/node_handle.h"
#include <ros/ros.h>

// Dynamic reconfigure
#include <dynamic_reconfigure/server.h>
#include <control_toolbox/ParametersConfig.h>

class TiXmlElement;

Expand Down Expand Up @@ -246,7 +250,19 @@ class Pid
*/
ROS_DEPRECATED double updatePid(double error, double error_dot, ros::Duration dt);

/**
* \brief Update the PID parameters from dynamics reconfigure
*/
void parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level);

/**
* @brief Start the dynamic reconfigure node and load the default values
*/
void initDynamicReconfigure(ros::NodeHandle &node);

/**
* @brief Custom assignment operator
*/
Pid &operator =(const Pid& p)
{
if (this == &p)
Expand All @@ -273,6 +289,14 @@ class Pid
double i_max_; /**< Maximum allowable integral term. */
double i_min_; /**< Minimum allowable integral term. */
double cmd_; /**< Command to send. */

// Dynamics reconfigure
boost::shared_ptr< dynamic_reconfigure::Server
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How about typedef-ing dynamic_reconfigure::Server<control_toolbox::ParametersConfig>?. It's a long typename used in quite a few places. Maybe to something like DynamicReconfigureServer

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done, named DynamicReconfigServer

<control_toolbox::ParametersConfig> > param_reconfigure_srv_;
dynamic_reconfigure::Server<
control_toolbox::ParametersConfig>::CallbackType param_reconfigure_callback_;
boost::recursive_mutex param_reconfigure_mutex_;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Apart from passing this mutex to the dynamic reconfigure server, shouldn't you be using it to prevent concurrent access of the gains by updateCommand(...) and parameterReconfigureCallback(...)?. In ros_control a single-threaded deployment is assumed, so no concurrency issues will arise; but this class might be used outside ros_control, and PID loop and gain updates might occur from different threads, right?.

On realtime systems mutices should be used with care, as you risk priority inversions. So the updateCommand(...) or any other realtime-safe code block should never have to wait on a shared resource. If this can be guaranteed, you're good to go.

Another thing to keep an eye on is making Linux calls from realtime code (allocations, printf, certain time queries, etc.), but this is not the case in your PR.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yea, dynamic reconfigure throws warnings if a shared mutex is not provided to it at initialization, but I wasn't sure where else to use the mutex and could not find documentation on this. There is only one location in the Pid code where it updates the values of the dynamic reconfigure parameters, and I had experimentally done this:

param_reconfig_mutex_.lock();
param_reconfig_server_->updateConfig(config);
param_reconfig_mutex_.unlock();
``

Does that look right? This is only called 1) when Pid is initialized and 2) when a process calls the ``setGains()`` function of Pid.


};

}
Expand Down
6 changes: 5 additions & 1 deletion package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,8 @@
<license>BSD</license>

<url type="website">http://ros.org/wiki/control_toolbox</url>
<!-- <url type="bugtracker"></url> -->
<url type="bugtracker">https://github.com/ros-controls/control_toolbox/issues</url>
<url type="repository">https://github.com/ros-controls/control_toolbox/</url>

<author>Melonee Wise</author>
<author>Sachin Chitta</author>
Expand All @@ -20,12 +21,15 @@
<build_depend>roscpp</build_depend>
<build_depend>angles</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>tinyxml</build_depend>

<run_depend>rosconsole</run_depend>
<run_depend>tf</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>angles</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>tinyxml</run_depend>

<export>
Expand Down
61 changes: 57 additions & 4 deletions src/pid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

// Original version: Melonee Wise <mwise@willowgarage.com>
/*
Author: Melonee Wise
Contributors: Jonathan Bohren, Bob Holmberg, Wim Meeussen, Dave Coleman
Desc: Implements a standard proportional-integral-derivative controller
*/

#include "control_toolbox/pid.h"
#include "tinyxml.h"
Expand Down Expand Up @@ -99,28 +103,77 @@ bool Pid::initXml(TiXmlElement *config)
i_min_ = -std::abs(i_clamp);

reset();

// Create node handle for dynamic reconfigure
std::string prefix = "pid"; // \todo make better default prefix somehow?
ros::NodeHandle nh(prefix);
initDynamicReconfigure(nh);

return true;
}

bool Pid::init(const ros::NodeHandle &node)
{
ros::NodeHandle n(node);
if (!n.getParam("p", p_gain_)) {

// Load PID gains from parameter server
if (!n.getParam("p", p_gain_))
{
ROS_ERROR("No p gain specified for pid. Namespace: %s", n.getNamespace().c_str());
return false;
}
n.param("i", i_gain_, 0.0);
n.param("d", d_gain_, 0.0);
if (!n.getParam("i", i_gain_))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

init should not fail when the derivative or integral gains are not specified. The previous behavior of defaulting them to zero without raising an error should be preserved. We should allow specifying P, PD or PI controllers without requiring the explicit setting of the unused gains. The proportional gain is error-checked because it is unlikely you'll find a controller without it.

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ah, that makes a lot of sense. I thought it had just been coding laziness!

{
ROS_ERROR("No i gain specified for pid. Namespace: %s", n.getNamespace().c_str());
return false;
}
if (!n.getParam("d", d_gain_))
{
ROS_ERROR("No d gain specified for pid. Namespace: %s", n.getNamespace().c_str());
return false;
}

// Load integral clamp from param server or default to 0
double i_clamp;
n.param("i_clamp", i_clamp, 0.0);
i_max_ = std::abs(i_clamp);
i_min_ = -std::abs(i_clamp);

reset();

initDynamicReconfigure(n);

return true;
}

void Pid::initDynamicReconfigure(ros::NodeHandle &node)
{
ROS_DEBUG_STREAM_NAMED("pid","Initializing dynamic reconfigure in namespace "
<< node.getNamespace());

// Start dynamic reconfigure server
param_reconfigure_srv_.reset(new dynamic_reconfigure::Server
<control_toolbox::ParametersConfig>(param_reconfigure_mutex_, node));

// Get starting values
control_toolbox::ParametersConfig config;

getGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);

// Set starting values
param_reconfigure_srv_->updateConfig(config);

// Set callback
param_reconfigure_callback_ = boost::bind(&Pid::parameterReconfigureCallback, this, _1, _2);
param_reconfigure_srv_->setCallback(param_reconfigure_callback_);
}

void Pid::parameterReconfigureCallback(control_toolbox::ParametersConfig &config, uint32_t level)
{
ROS_DEBUG_STREAM_NAMED("pid","Dynamics reconfigure callback recieved.");
setGains(config.p_gain, config.i_gain, config.d_gain, config.i_clamp_max, config.i_clamp_min);
}

double Pid::computeCommand(double error, ros::Duration dt)
{
double p_term, d_term;
Expand Down