ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
3

getParam a nested std::map

asked 2017-07-11 05:42:56 -0500

rook gravatar image

I am writing a node that will read a YAML file (loaded from roslaunch) with the following structure:

home:
  1:
    "x": 0.0
    "y": 0.0
    "z": 0.0
    "qx": 0.0
    "qy": 0.0
    "qz": 0.0
    "qw": 0.0
  2:
    "x": 0.0
    "y": 0.0
    "z": 0.0
    "qx": 0.0
    "qy": 0.0
    "qz": 0.0
    "qw": 0.0

This is the relevant code on the node side:

class RouteParserNode
{
    public:
        RouteParserNode() :
            nh_private_("~")
        {
            nh_private_.getParam("home", route_home);
        };
        ~RouteParserNode();
     private:
        std::map<int, std::map<std::string, double>> route_home;
        ros::NodeHandle nh_private_;
};

However, the above node cannot be compiled and gives the following error:

error: no matching function for call to ‘ros::NodeHandle::getParam(const char 
[5], std::map<int, std::map<std::basic_string<char>, double> >&)’
             nh_private_.getParam("home", route_home);

which I assume is because getParam does not work for nested std::maps.

I am currently on ROS Indigo.

Is there a workaround for this, or another way for me to parse the file? My ultimate goal is to supply a list of goals (geometry_msgs/PoseStamped, but only primitive types were allowed for the std::map method hence the above yaml structure).

edit retag flag offensive close merge delete

Comments

#q197304 is very similar but the answer doesn't provide a good code example.

lucasw gravatar image lucasw  ( 2017-07-11 08:26:57 -0500 )edit

1 Answer

Sort by » oldest newest most voted
1

answered 2017-07-11 08:59:54 -0500

lucasw gravatar image

updated 2017-07-11 09:40:47 -0500

For C++ there is an answer involving XmlRpc that #q197304 suggests, I used #q189299 as a starting point:

XmlRpc::XmlRpcValue list1;
nh.getParam("/home", list1);
for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = list1.begin();
    it != list1.end(); ++it)
{ 
  ROS_INFO_STREAM(it->first);
  XmlRpc::XmlRpcValue list2 = it->second;
  for(XmlRpc::XmlRpcValue::ValueStruct::const_iterator it2 = list2.begin();
      it2 != list2.end(); ++it2)
  { 
    XmlRpc::XmlRpcValue xml_rpc_value = it2->second;
    if (xml_rpc_value.getType() != XmlRpc::XmlRpcValue::TypeDouble)
      continue;

    const double value = static_cast<double>(xml_rpc_value);
    ROS_INFO_STREAM(it2->first << " " << value);
  }
}

What is maybe less efficient but uses more familiar standard C++ string operations instead of xmlrpc is to use getParamNames to get all the parameters then search through them for the tree you want ('home') and populate your map of maps manually.

  std::vector<std::string> keys;
  nh.getParamNames(keys);

  // or provide via param, or ros::this_node::getNamespace();
  std::string base_namespace = "/home";   // don't forget the leading slash

  ROS_INFO_STREAM(ns);

  for (size_t i = 0; i < keys.size(); ++i)
  {
    ... if keys[i] doesn't have a substring match of base_namespace then continue
    ... remove base_namespace from keys[i]
    ... split keys[i] into a vector of strings using '/' as a delimiter
    ... test if there are more or less split keys than expected (!= 3)
    // populate the nested maps 
    ... create route_home[split_keys[0]] if it doesn't already exist, then
    route_home[split_keys[0]][split_keys[1]] = split_keys[2];
  }

I get "ERROR: YAML dictionaries must have string keys" so I have to use "1" and "2" instead of 1 and 2 for keys for your example.

It would be nice if there was a nh.getParamNames(keys, "/foo"); and it would only get the params under '/foo'.

edit flag offensive delete link more

Comments

1

To add to this: you might take a look at rosparam_shortcuts by Dave Coleman. It includes functionality similar to this.

See the api doc fi.

gvdhoorn gravatar image gvdhoorn  ( 2017-07-11 10:08:31 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2017-07-11 05:41:46 -0500

Seen: 2,799 times

Last updated: Jul 11 '17