ROS Dynamic Reconfigure

ddynamic_reconfigure.py

#!/usr/bin/env python

"""
Dynamic dynamic reconfigure server.

Just register your variables for the dynamic reconfigure
and call start with a callback.

Author: Sammy Pfeiffer
"""

from dynamic_reconfigure.server import Server
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator
from dynamic_reconfigure.encoding import extract_params
from rospkg import RosPack
import rospy


class DDynamicReconfigure(ParameterGenerator):
    """Dynamic reconfigure server that can be instanced directly."""

    def __init__(self, name=None):
        global id
        self.group = self.Group(self, "Default", "", True, 0, 0)
        id = 1
        if name is None:
            self.name = rospy.get_name() + "_dyn_rec"
        else:
            self.name = name
        self.constants = []
        rp = RosPack()
        self.dynconfpath = rp.get_path('dynamic_reconfigure')

    def get_type(self):
        class TypeClass(object):
            def __init__(self, config_description):
                self.config_description = config_description
                self.min = {}
                self.max = {}
                self.defaults = {}
                self.level = {}
                self.type = {}
                self.all_level = 0

                for param in extract_params(config_description):
                    self.min[param['name']] = param['min']
                    self.max[param['name']] = param['max']
                    self.defaults[param['name']] = param['default']
                    self.level[param['name']] = param['level']
                    self.type[param['name']] = param['type']
                    self.all_level = self.all_level | param['level']
        return TypeClass(self.group.to_dict())

    def add_variable(self, name, description, default=None, min=None, max=None, edit_method=""):
        """Register variable, like gen.add() but deducting the type"""
        if type(default) == int:
            if edit_method == "":
                self.add(name, "int", 0, description, default, min, max)
            else: # enum
                self.add(name, "int", 0, description, default, min, max, edit_method)
        elif type(default) == float:
            self.add(name, "double", 0, description, default, min, max)
        elif type(default) == str:
            self.add(name, "str", 0, description, default)
        elif type(default) == bool:
            self.add(name, "bool", 0, description, default)

        return default


    def get_variable_names(self):
        """Return the names of the dynamic reconfigure variables"""
        names = []
        for param in self.group.parameters:
            names.append(param['name'])
        return names


    def start(self, callback):
        self.dyn_rec_srv = Server(self.get_type(), callback, namespace=self.name)


class DyConfigure(object):
    def __init__(self):
        # Create a D(ynamic)DynamicReconfigure
        self.ddr = DDynamicReconfigure("class_example")

        # Add variables (name, description, default value, min, max, edit_method)
        self.ddr.add_variable("decimal", "float/double variable", 0.0, -1.0, 1.0)
        self.ddr.add_variable("dy_eps", "DBSCAN eps parameter", 1.0, -1.0, 1.0)
        self.ddr.add_variable("integer", "integer variable", 0, -1, 1)
        self.ddr.add_variable("bool", "bool variable", True)
        self.ddr.add_variable("string", "string variable", "string dynamic variable")
        enum_method = self.ddr.enum([ self.ddr.const("Small",      "int", 0, "A small constant"),
                           self.ddr.const("Medium",     "int", 1, "A medium constant"),
                           self.ddr.const("Large",      "int", 2, "A large constant"),
                           self.ddr.const("ExtraLarge", "int", 3, "An extra large constant")],
                         "An enum example")
        self.ddr.add_variable("enumerate", "enumerate variable", 1, 0, 3, edit_method=enum_method)

        self.add_variables_to_self()

        self.ddr.start(self.dyn_rec_callback)

    def add_variables_to_self(self):
        var_names = self.ddr.get_variable_names()
        for var_name in var_names:
            self.__setattr__(var_name, None)

    def dyn_rec_callback(self, config, level):
        rospy.loginfo("Received reconf call: " + str(config))
        # Update all variables
        var_names = self.ddr.get_variable_names()
        for var_name in var_names:
            self.__dict__[var_name] = config[var_name]
        return config