bool get(std::string key, XmlRpc::XmlRpcValue& val)
{
try
{
bool v;
if (get(key,v)){
XmlRpc::XmlRpcValue vb(v);
val = vb;
return (val.valid());
}
double vd;
if (get(key,vd)){
val = vd;
return (val.valid());
}
int vi;
if (get(key,vi)){
val=vi;
return (val.valid());
}
std::string vs;
if (get(key, vs)){
val=vs.c_str();
return (val.valid());
}
}
catch (...)
{
return false;
}
return false;
}
void startWorkingCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
// if no arguments were received
if (!params.valid())
{
result = ctrl_xmlrpc->startProcessing();
}
else
{
switch(params.size())
{
// start the server in online mode with port and blocksize
case 2:
ctrl_xmlrpc->startEegServer((int) params[0], (int) params[1]);
result = 0;
break;
case 3:
ctrl_xmlrpc->startEegServer((std::string) params[0], (int) params[1], (int) params[2]);
result = 0;
break;
default:
OMG("WARNING! Server got wrong number of arguments!");
break;
}
}
fflush(stdout);
}
void applySetupCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
if(params.valid()) {
if(params.size() != 1) {
result = -1;
return;
}
XmlRpc::XmlRpcValue p = params[0];
if(p.size() % 2 != 0) {
result = -1;
fflush(stdout);
return;
}
for(int i=0; i<p.size(); i+= 2) {
if(0 > ctrl_xmlrpc->addModule(p[i], p[i+1])) {
result = i+1;
fflush(stdout);
return;
}
}
}
result = 0;
fflush(stdout);
return;
}
void loadMonocularNodelets(nodelet::Loader& manager, const std::string& side,
const XmlRpc::XmlRpcValue& rectify_params,
const nodelet::V_string& my_argv)
{
nodelet::M_string remappings;
// Explicitly resolve global remappings (wg-ros-pkg #5055).
// Otherwise the internal remapping 'image_raw' -> 'left/image_raw' can hide a
// global remapping from the launch file or command line.
std::string image_raw_topic = ros::names::resolve(side + "/image_raw");
std::string image_mono_topic = ros::names::resolve(side + "/image_mono");
std::string image_color_topic = ros::names::resolve(side + "/image_color");
std::string image_rect_topic = ros::names::resolve(side + "/image_rect");
std::string image_rect_color_topic = ros::names::resolve(side + "/image_rect_color");
std::string camera_info_topic = ros::names::resolve(side + "/camera_info");
// Debayer nodelet: image_raw -> image_mono, image_color
remappings["image_raw"] = image_raw_topic;
remappings["image_mono"] = image_mono_topic;
remappings["image_color"] = image_color_topic;
std::string debayer_name = ros::this_node::getName() + "_debayer_" + side;
manager.load(debayer_name, "image_proc/debayer", remappings, my_argv);
// Rectify nodelet: image_mono -> image_rect
remappings.clear();
remappings["image_mono"] = image_mono_topic;
remappings["camera_info"] = camera_info_topic;
remappings["image_rect"] = image_rect_topic;
std::string rectify_mono_name = ros::this_node::getName() + "_rectify_mono_" + side;
if (rectify_params.valid())
ros::param::set(rectify_mono_name, rectify_params);
manager.load(rectify_mono_name, "image_proc/rectify", remappings, my_argv);
// Rectify nodelet: image_color -> image_rect_color
remappings.clear();
remappings["image_mono"] = image_color_topic;
remappings["camera_info"] = camera_info_topic;
remappings["image_rect"] = image_rect_color_topic;
std::string rectify_color_name = ros::this_node::getName() + "_rectify_color_" + side;
if (rectify_params.valid())
ros::param::set(rectify_color_name, rectify_params);
manager.load(rectify_color_name, "image_proc/rectify", remappings, my_argv);
}
void addModuleCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
result = 0;
if (params.valid())
{
result = ctrl_xmlrpc->addModule((std::string) params[0], (std::string) params[1]);
} else {
FYI("Error reading XMLRPC Message..");
}
fflush(stdout);
}
void getUsageCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
if(!params.valid()) {
FYI("Invalid Params from XMLRPC");
result = -1;
return;
}
Module* temp = ModuleFactory::createInstance((std::string)params[0]);
if(temp == NULL) {
FYI("Could not create Object of %s", ((std::string)params[0]).c_str());
result = -1;
return;
}
result.setSize(4);
// the usage string
result[0] = temp->usage();
// input type
result[1] = temp->inputType();
// output type
result[2] = temp->outputType();
// input, output, or flow node?
if(!temp->isInput() && !temp->isOutput()) {
result[3] = std::string("flow");
} else if(temp->isInput()) {
result[3] = std::string("input");
} else {
result[3] = std::string("output");
}
delete temp;
fflush(stdout);
return;
}
void startLoopbackCallback::execute (XmlRpc::XmlRpcValue& params, XmlRpc::XmlRpcValue& result)
{
// if no arguments were received
if (!params.valid())
{
ctrl_xmlrpc->startProcessing();
}
else
{
switch(params.size())
{
case 3:
FYI("params[0]: %d\n\tparams[1]: %s\n\tparams[2]: %d\n\t", (int)params[0], ((std::string)params[1]).c_str(), (int)params[2]);
ctrl_xmlrpc->startLoopback((int)params[0], (std::string) params[1], (int) params[2]);
break;
default:
WTF("WARNING! Server got wrong number of arguments!");
break;
}
}
fflush(stdout);
result = 0;
}