Skip to content

Instantly share code, notes, and snippets.

@v4hn
Last active April 21, 2020 14:30
Show Gist options
  • Save v4hn/15da2a4c3fbdb534fba98dc4526a3852 to your computer and use it in GitHub Desktop.
Save v4hn/15da2a4c3fbdb534fba98dc4526a3852 to your computer and use it in GitHub Desktop.
Hijack cached parameters for dynamic parameter updates without dynamic_reconfigure
#include <ros/ros.h>
#include <ros/xmlrpc_manager.h>
const std::string PARAMETER {"test_parameter"};
namespace ros {
std::set<std::string> g_subscribed_params;
namespace param {
void paramUpdateCallback(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result);
}
}
void paramUpdate(XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result){
ROS_INFO_STREAM("paramUpdate called: " << static_cast<std::string>(params[1]));
if(static_cast<std::string>(params[1]) == PARAMETER)
ROS_INFO_STREAM(PARAMETER << ": updated to new value " << static_cast<std::string>(params[2]));
ros::param::paramUpdateCallback(params, result);
}
int main(int argc, char** argv){
ros::init(argc, argv, "parameter_subscriber");
ros::NodeHandle nh{};
std::string param{};
nh.getParamCached(PARAMETER, param);
ros::XMLRPCManager::instance()->bind("paramUpdate", paramUpdate);
ros::Rate r(1);
while(ros::ok()){
ROS_INFO_STREAM(PARAMETER << ": '" << param << "' / subscribed_params: " << ros::g_subscribed_params.size());
nh.getParamCached(PARAMETER, param);
ros::spinOnce();
r.sleep();
}
return 0;
}
@v4hn
Copy link
Author

v4hn commented Apr 21, 2020

Unfortunately, I could not get it to work yet, because apparently the subscription for the parameter fails to set up.

@v4hn
Copy link
Author

v4hn commented Apr 21, 2020

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment