Skip to content

Commit

Permalink
Merge branch 'add-dynamic-parameters' of https://github.com/matt-atta…
Browse files Browse the repository at this point in the history 
…ck/marti_common into add-dynamic-parameters
  • Loading branch information
Matthew Bries committed Jan 3, 2019
2 parents 01a19c6 + d168cbe commit 6f7b8ed
Show file tree
Hide file tree
Showing 2 changed files with 129 additions and 57 deletions.
138 changes: 81 additions & 57 deletions swri_roscpp/include/swri_roscpp/dynamic_parameters.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,24 +30,24 @@ namespace swri
Type type;
std::string name;
std::string description;
void* pointer;

void* pointer;//pointer to the parameter to update on change

// Defaults, maximum and minimum values for this parameter
union
{
double d;
bool b;
int i;
//std::string s;
} Default;
union
{
double d;
bool b;
int i;
} Min;
union
{
double d;
bool b;
int i;
} Max;
std::string default_string;// to get around union issues with strings
Expand Down Expand Up @@ -161,13 +161,65 @@ namespace swri
*v = param.value;
}

PublishCurrent(rsp.config);
updateCurrent(rsp.config);

return true;
}

// Updates a config with the current parameter values
void updateCurrent(dynamic_reconfigure::Config& config)
{
for (std::map<std::string, DynamicValue>::iterator value = values_.begin(); value != values_.end(); value++)
{
if (value->second.type == DynamicValue::Double)
{
dynamic_reconfigure::DoubleParameter param;
param.name = value->first;
param.value = *(double*)value->second.pointer;
config.doubles.push_back(param);
}
else if (value->second.type == DynamicValue::Float)
{
dynamic_reconfigure::DoubleParameter param;
param.name = value->first;
param.value = *(float*)value->second.pointer;
config.doubles.push_back(param);
}
else if (value->second.type == DynamicValue::Int)
{
dynamic_reconfigure::IntParameter param;
param.name = value->first;
param.value = *(int*)value->second.pointer;
config.ints.push_back(param);
}
else if (value->second.type == DynamicValue::Bool)
{
dynamic_reconfigure::BoolParameter param;
param.name = value->first;
param.value = *(bool*)value->second.pointer;
config.bools.push_back(param);
}
else if (value->second.type == DynamicValue::String)
{
dynamic_reconfigure::StrParameter param;
param.name = value->first;
param.value = *(std::string*)value->second.pointer;
config.strs.push_back(param);
}
}

dynamic_reconfigure::GroupState gs;
gs.name = "Default";
gs.state = true;
gs.id = 0;
gs.parent = 0;
config.groups.push_back(gs);
update_pub_.publish(config);
}

public:

// Sets up the node handle and publishers. Be sure to call this before finalize or any of the 'get*' calls.
void initialize(ros::NodeHandle& pnh)
{
boost::mutex::scoped_lock lock(mutex_);
Expand All @@ -180,6 +232,7 @@ namespace swri
&DynamicParameters::setConfigCallback, this);
}

// Publishes the configuration parameters that have been added
void finalize()
{
// publish the configs as one group
Expand Down Expand Up @@ -278,62 +331,35 @@ namespace swri
descr_pub_.publish(rdesc);

dynamic_reconfigure::Config config;
PublishCurrent(config);
updateCurrent(config);
}

void PublishCurrent(dynamic_reconfigure::Config& config)
boost::mutex& mutex()
{
for (std::map<std::string, DynamicValue>::iterator value = values_.begin(); value != values_.end(); value++)
{
if (value->second.type == DynamicValue::Double)
{
dynamic_reconfigure::DoubleParameter param;
param.name = value->first;
param.value = *(double*)value->second.pointer;
config.doubles.push_back(param);
}
else if (value->second.type == DynamicValue::Float)
{
dynamic_reconfigure::DoubleParameter param;
param.name = value->first;
param.value = *(float*)value->second.pointer;
config.doubles.push_back(param);
}
else if (value->second.type == DynamicValue::Int)
{
dynamic_reconfigure::IntParameter param;
param.name = value->first;
param.value = *(int*)value->second.pointer;
config.ints.push_back(param);
}
else if (value->second.type == DynamicValue::Bool)
{
dynamic_reconfigure::BoolParameter param;
param.name = value->first;
param.value = *(bool*)value->second.pointer;
config.bools.push_back(param);
}
else if (value->second.type == DynamicValue::String)
{
dynamic_reconfigure::StrParameter param;
param.name = value->first;
param.value = *(std::string*)value->second.pointer;
config.strs.push_back(param);
}
}

dynamic_reconfigure::GroupState gs;
gs.name = "Default";
gs.state = true;
gs.id = 0;
gs.parent = 0;
config.groups.push_back(gs);
update_pub_.publish(config);
return mutex_;
}

boost::mutex& mutex()
inline
void get(const std::string &name,
float &variable,
const float default_value,
const std::string description = "None.",
const float min = -100,
const float max = 100)
{
return mutex_;
DynamicValue value;
value.type = DynamicValue::Float;
value.description = description;
value.Min.d = min;
value.Max.d = max;
value.Default.d = default_value;
value.pointer = (void*)&variable;
values_[name] = value;

std::string resolved_name = nh_.resolveName(name);
//_used_params.insert(resolved_name);
nh_.param(name, variable, default_value);
ROS_INFO("Read dynamic parameter %s = %f", name.c_str(), variable);
}

inline
Expand Down Expand Up @@ -391,8 +417,6 @@ namespace swri
DynamicValue value;
value.type = DynamicValue::Bool;
value.description = description;
value.Min.b = false;
value.Max.b = true;
value.Default.b = default_value;
value.pointer = (void*)&variable;
values_[name] = value;
Expand Down
48 changes: 48 additions & 0 deletions swri_roscpp/readme.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,3 +31,51 @@ add_topic_service_files(DIRECTORY topic_srv FILES
```

To use the generated topics simply include `your_project/your_service.h`. They have response and request fields just like normal services.

## Dynamic Parameters

A class that implements the same functionality as Dynamic Reconfigure, but in a more dynamic and easier to use way. Instead of declaring configurable parameters at compile time in a config file they are specified in your C++ code.

To use, add the class to your node and call the initalize function with a NodeHandle in your private namespace.

```cpp
#include <swri_roscpp/dynamic_parameters.h>

void main()
{
// your node stuff here

ros::NodeHandle pnh("~");
swri::DynamicParameters params;
params.initialize(pnh);
...
```
You can then start declaring and reading in configuration values, giving a long living pointer to a variable for each. This variable is changed on a dynamic reconfigure of that parameter. If you want to ensure no changes are applied during an operation, be sure to lock the mutex you can get by calling the `params.mutex()` function.
```cpp
float flt;
params.get("float_value", flt,
10.0f /*default*/, "Description...",
-10.0f /* min */, 10.0f /* max */);
double dbl;
params.get("double_value", dbl,
10.0 /*default*/, "Description...",
-10.0 /* min */, 10.0 /* max */);
bool bl;
params.get("bool_value", bl,
10.0f /*default*/, "Description...");
std::string str;
params.get("string_value", str,
"default" /*default*/, "Description...");
```

These functions read in the current parameter value to the provided variable, if they haven't been set they are set to the default value. After you have read in all the variables you want to be dynamically reconfigurable, call the `finalize()` function.

```cpp
params.finalize();
```

This publishes the configuration options to a latched topic so that the dynamic reconfigure tools can read them in.

After you call finalize, you should be able to access and dynamically configure your node with any dynamic reconfigure compatible tools.

0 comments on commit 6f7b8ed

Please sign in to comment.