Skip to content

Instantly share code, notes, and snippets.

@adioshun
Created November 2, 2018 07:32
Show Gist options
  • Save adioshun/07d78f1400cc80b66950494263883482 to your computer and use it in GitHub Desktop.
Save adioshun/07d78f1400cc80b66950494263883482 to your computer and use it in GitHub Desktop.
ROS Dynamic Reconfigure
#!/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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment