Added dynamic reconfigure for PID gains

CMakeLists.txt

@@ -6,30 +6,47 @@ if(USE_ROSBUILD)
   rosbuild_init()
   #rosbuild_genmsg()
   rosbuild_gensrv()
-  rosbuild_add_library(control_toolbox
+  rosbuild_add_library(${PROJECT_NAME}
     src/pid.cpp
     src/pid_gains_setter.cpp
     src/sine_sweep.cpp
     src/dither.cpp
     src/sinusoid.cpp
     src/limited_proxy.cpp
     )
-  target_link_libraries(control_toolbox tinyxml)
+  target_link_libraries(${PROJECT_NAME} tinyxml)
 
   # rosbuild_add_executable(test_linear test/linear.cpp)
 
   # Tests
-  rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp) 
-  target_link_libraries(test/pid_tests control_toolbox)
+  rosbuild_add_gtest(test/pid_tests test/pid_tests.cpp)
+  target_link_libraries(test/pid_tests ${PROJECT_NAME})
 
 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
+    realtime_tools
+    )
+
+  find_package(Boost REQUIRED COMPONENTS system thread)
 
   include_directories(
-    include 
-    ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+    include
+    ${catkin_INCLUDE_DIRS}
+    ${Boost_INCLUDE_DIR}
+    )
+
+  # Dynamics reconfigure
+  generate_dynamic_reconfigure_options(
+    cfg/Parameters.cfg
+    )
 
   # Add services and generate them
   add_service_files(
@@ -41,6 +58,20 @@ else()
     DEPENDENCIES std_msgs
     )
 
+  # Declare catkin package
+  catkin_package(
+    DEPENDS tinyxml
+    CATKIN_DEPENDS 
+      rosconsole 
+      tf 
+      roscpp 
+      angles 
+      dynamic_reconfigure
+      realtime_tools
+    INCLUDE_DIRS include
+    LIBRARIES ${PROJECT_NAME}
+    )
+
   add_library(${PROJECT_NAME}
     src/pid.cpp
     src/pid_gains_setter.cpp
@@ -49,20 +80,14 @@ else()
     src/sinusoid.cpp
     src/limited_proxy.cpp
     )
+  add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencfg) # wait for dynamic reconfigure
 
-  target_link_libraries(control_toolbox tinyxml ${catkin_LIBRARIES})
-  add_dependencies(control_toolbox ${PROJECT_NAME}_gencpp)
+  target_link_libraries(${PROJECT_NAME} tinyxml ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
-  # Declare catkin package
-  catkin_package(
-    DEPENDS tinyxml
-    CATKIN_DEPENDS rosconsole tf roscpp angles 
-    INCLUDE_DIRS include 
-    LIBRARIES ${PROJECT_NAME} 
-    )
+  add_dependencies(${PROJECT_NAME} ${PROJECT_NAME}_gencpp) # wait for msgs
 
   # Tests
-  catkin_add_gtest(pid_tests test/pid_tests.cpp) 
+  catkin_add_gtest(pid_tests test/pid_tests.cpp)
   target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME})
 
   # add_executable(test_linear test/linear.cpp)
@@ -78,6 +103,6 @@ else()
     )
 
   install(DIRECTORY scripts/
-     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+    DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 endif()

cfg/Parameters.cfg

@@ -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)
+                 # PkgName  #NodeName         #Prefix for generated .h include file, e.g. ParametersConfig.py
+exit(gen.generate(PACKAGE, "control_toolbox", "Parameters"))