Added LeadLag controller

.travis.yml

@@ -1,48 +1,32 @@
-# Travis Continuous Integration Configuration File For ROS Control Projects
-# Author: Dave Coleman
-language:
- - cpp
- - python
-python:
- - "2.7"
+# This config file for Travis CI utilizes ros-industrial/industrial_ci package.
+# For more info for the package, see https://github.com/ros-industrial/industrial_ci/blob/master/README.rst
+sudo: required 
+dist: trusty 
+services:
+ - docker
+language: generic 
 compiler:
  - gcc
 notifications:
  email:
+ on_success: change
+ on_failure: change
  recipients:
  - davetcoleman@gmail.com
  - adolfo.rodriguez@pal-robotics.com
- on_success: change #[always|never|change] # default: change
- on_failure: change #[always|never|change] # default: always
-before_install: # Use this to prepare the system to install prerequisites or dependencies
- # Define some config vars
- - export ROS_DISTRO=hydro
- - export CI_SOURCE_PATH=$(pwd)
- - export REPOSITORY_NAME=${PWD##*/}
- - echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME"
- - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list'
- - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
- - sudo apt-get update -qq
- - sudo apt-get install -qq -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin ros-$ROS_DISTRO-ros
- # Setup rosdep
- - sudo rosdep init
- - rosdep update
-install: # Use this to install any prerequisites or dependencies necessary to run your build
- # Create workspace
- - mkdir -p ~/ros/ws_ros_controls/src
- - cd ~/ros/ws_ros_controls/src
- - wstool init .
- # Download non-debian stuff
- - wstool merge https://raw.github.com/ros-controls/ros_control/$ROS_DISTRO-devel/ros_control.rosinstall
- - wstool update
- # Delete the ros_control.rosinstall version of this repo and use the one of the branch we are testing
- - rm -rf $REPOSITORY_NAME
- - ln -s $CI_SOURCE_PATH . # Link the repo we are testing to the new workspace
- - cd ../
- # Install dependencies for source repos
- - rosdep install --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
-before_script: # Use this to prepare your build for testing e.g. copy database configurations, environment variables, etc.
- - source /opt/ros/$ROS_DISTRO/setup.bash 
-script: # All commands must exit with code 0 on success. Anything else is considered failure.
- - catkin_make -j2
-
+ - enrique.fernandez.perdomo@gmail.com
+ - bence.magyar.robotics@gmail.com 
+ - gm130s@gmail.com
+env:
+ global:
+ - CATKIN_PARALLEL_TEST_JOBS=-p1
+ - ROS_PARALLEL_TEST_JOBS=-j1
+ matrix:
+ - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros/ubuntu
+ - ROS_DISTRO="indigo" ROS_REPOSITORY_PATH=http://packages.ros.org/ros-shadow-fixed/ubuntu
+ - ROS_DISTRO="indigo" UPSTREAM_WORKSPACE=https://raw.githubusercontent.com/ros-controls/ros_control/$ROS_DISTRO-devel/ros_control.rosinstall
+install:
+ - git clone https://github.com/ros-industrial/industrial_ci.git .ci_config
+script:
+ - source .ci_config/travis.sh 
+## - source ./travis.sh # Enable this when you have a package-local script 

CMakeLists.txt

@@ -17,12 +17,14 @@ find_package(TinyXML REQUIRED)
 # Dynamics reconfigure
 generate_dynamic_reconfigure_options(
  cfg/Parameters.cfg
+ cfg/LeadLag.cfg
  )
 
 # Add services and generate them
 add_service_files(
  FILES
  SetPidGains.srv
+ SetLeadLagGains.srv
  )
 
 generate_messages(
@@ -50,6 +52,8 @@ include_directories(
  )
 
 add_library(${PROJECT_NAME}
+ src/lead_lag.cpp
+ src/lead_lag_gains_setter.cpp
  src/pid.cpp
  src/pid_gains_setter.cpp
  src/sine_sweep.cpp
@@ -68,6 +72,9 @@ if(CATKIN_ENABLE_TESTING)
  catkin_add_gtest(pid_tests test/pid_tests.cpp)
  target_link_libraries(pid_tests ${catkin_LIBRARIES} ${PROJECT_NAME})
 
+ catkin_add_gtest(lead_lag_tests test/lead_lag_tests.cpp)
+ target_link_libraries(lead_lag_tests ${catkin_LIBRARIES} ${PROJECT_NAME})
+
  # add_executable(test_linear test/linear.cpp)
 endif()
 

cfg/LeadLag.cfg

@@ -0,0 +1,62 @@
+#! /usr/bin/env python
+#*********************************************************************
+#* Software License Agreement (BSD License)
+#*
+#* Copyright (c) 2017, 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: Aris Synodinos
+# Desc: Allows Lead Lag parameters, etc to be tuned in realtime using dynamic reconfigure
+
+PACKAGE='control_toolbox'
+
+def generate(gen):
+ # Name Type Level Description Default Min Max
+ gen.add( "k" , double_t, 1,"Gain.", 10.0 , -100000 , 100000)
+ gen.add( "a" , double_t, 1,"Lead gain.", 0.1 , -1000 , 1000)
+ gen.add( "b" , double_t, 1,"Lag gain.", 1.0 , -1000 , 1000)
+ # PkgName #NodeName #Prefix for generated .h include file, e.g. ParametersConfig.py
+ exit(gen.generate(PACKAGE, "control_toolbox", "LeadLag"))
+
+# try catkin generator first
+try:
+ from dynamic_reconfigure.parameter_generator_catkin import *
+ gen = ParameterGenerator()
+ generate(gen)
+# reason for catching IndexError
+# parameter_generator_catkin expects 4 arguments while rosbuild only passes in 2
+# not thrilled with this solution
+except IndexError:
+ print 'ERROR', PACKAGE, 'Parameters.cfg failed using parameter_generator_catkin, using rosbuild instead'
+ from dynamic_reconfigure.parameter_generator import *
+ gen = ParameterGenerator()
+ generate(gen)

doc.h

@@ -4,9 +4,11 @@
 
 The control_toolbox package contains the following C++ classes which may be useful when implementing controllers.
  \li \ref control_toolbox::Pid "Pid" for implementing position/integral/derivative control loops.
+ \li \ref control_toolbox::LeadLag "Lead Lag" for implementing lead or lag compensator control loops.
  \li \ref control_toolbox::Ramp "Ramp Output"
  \li \ref control_toolbox::SineSweep "Sine Sweep"
  \li \ref control_toolbox::Dither "Dither" for giving white noise at specified amplitude
  \li \ref control_toolbox::PidGainsSetter "Pid gains setter" for advertising a service for automatically changing PID gains.
+ \li \ref control_toolbox::LeadLagGainsSetter "Lead Lag gains setter" for advertising a service for automatically changing LeadLag gains.
 
 **/