C++ XmlRpcValue::size方法代码示例

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 Costmap2DROS::readFootprintFromXMLRPC( XmlRpc::XmlRpcValue& footprint_xmlrpc,
                                            const std::string& full_param_name )
{
  // Make sure we have an array of at least 3 elements.
  if( footprint_xmlrpc.getType() != XmlRpc::XmlRpcValue::TypeArray ||
      footprint_xmlrpc.size() < 3 )
  {
    ROS_FATAL( "The footprint must be specified as list of lists on the parameter server, %s was specified as %s",
               full_param_name.c_str(), std::string( footprint_xmlrpc ).c_str() );
    throw std::runtime_error( "The footprint must be specified as list of lists on the parameter server with at least 3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
  }

  std::vector<geometry_msgs::Point> footprint;  
  geometry_msgs::Point pt;

  for( int i = 0; i < footprint_xmlrpc.size(); ++i )
  {
    // Make sure each element of the list is an array of size 2. (x and y coordinates)
    XmlRpc::XmlRpcValue point = footprint_xmlrpc[ i ];
    if( point.getType() != XmlRpc::XmlRpcValue::TypeArray ||
        point.size() != 2 )
    {
      ROS_FATAL( "The footprint (parameter %s) must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.",
                 full_param_name.c_str() );
      throw std::runtime_error( "The footprint must be specified as list of lists on the parameter server eg: [[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form" );
    }
       
    pt.x = getNumberFromXMLRPC( point[ 0 ], full_param_name );
    pt.y = getNumberFromXMLRPC( point[ 1 ], full_param_name );
       
    footprint.push_back( pt );
  }

  setUnpaddedRobotFootprint( footprint );
}

  void NavSatTransform::run()
  {
    ros::Time::init();

    double frequency = 10.0;
    double delay = 0.0;

    ros::NodeHandle nh;
    ros::NodeHandle nhPriv("~");

    // Load the parameters we need
    nhPriv.getParam("magnetic_declination_radians", magneticDeclination_);
    nhPriv.param("yaw_offset", yawOffset_, 0.0);
    nhPriv.param("broadcast_utm_transform", broadcastUtmTransform_, false);
    nhPriv.param("zero_altitude", zeroAltitude_, false);
    nhPriv.param("publish_filtered_gps", publishGps_, false);
    nhPriv.param("use_odometry_yaw", useOdometryYaw_, false);
    nhPriv.param("wait_for_datum", useManualDatum_, false);
    nhPriv.param("frequency", frequency, 10.0);
    nhPriv.param("delay", delay, 0.0);

    // Subscribe to the messages and services we need
    ros::ServiceServer datumServ = nh.advertiseService("datum", &NavSatTransform::datumCallback, this);

    if (useManualDatum_ && nhPriv.hasParam("datum"))
    {
      XmlRpc::XmlRpcValue datumConfig;

      try
      {
        double datumLat;
        double datumLon;
        double datumYaw;

        nhPriv.getParam("datum", datumConfig);

        // Handle datum specification. Users should always specify a baseLinkFrameId_ in the
        // datum config, but we had a release where it wasn't used, so we'll maintain compatibility.
        ROS_ASSERT(datumConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);
        ROS_ASSERT(datumConfig.size() == 4 || datumConfig.size() == 5);

        useManualDatum_ = true;

        std::ostringstream ostr;
        if (datumConfig.size() == 4)
        {
          ROS_WARN_STREAM("No base_link_frame specified for the datum (parameter 5).");
          ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] << " " << datumConfig[3];
          std::istringstream istr(ostr.str());
          istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_;
        }
        else if (datumConfig.size() == 5)
        {
          ostr << datumConfig[0] << " " << datumConfig[1] << " " << datumConfig[2] <<
                  " " << datumConfig[3] << " " << datumConfig[4];
          std::istringstream istr(ostr.str());
          istr >> datumLat >> datumLon >> datumYaw >> worldFrameId_ >> baseLinkFrameId_;
        }

int main(int argc, char* argv[])
{
    std::vector<ros::Subscriber> subs;

    ros::init(argc, argv, "Navi2");
    ros::NodeHandle nh;
    
    XmlRpc::XmlRpcValue topicList;
    ros::param::get("~inputTopics", topicList);
    
    ROS_ASSERT(topicList.size() > 0);
    ROS_ASSERT(topicList.getType() == XmlRpc::XmlRpcValue::TypeArray);
    
    
    for (int i = 0; i < topicList.size(); ++i) 
    {
        ROS_ASSERT(topicList[i].getType() == XmlRpc::XmlRpcValue::TypeString);
        std::string topic = static_cast<std::string>(topicList[i]);
        
        maps.push_back(nav_msgs::OccupancyGrid());
        flags.push_back(0);
        
        subs.push_back(nh.subscribe<nav_msgs::OccupancyGrid>(topic, 1, boost::bind(mapCallback, _1, i)));
    }
    
    ros::Publisher mapPub = nh.advertise<nav_msgs::OccupancyGrid>("output", 1);
    ros::Rate rate(20);
    
    while (ros::ok())
    {
        rate.sleep();
        ros::spinOnce();
        
        //Everyone ready?
        if ((unsigned int)std::count(flags.begin(), flags.end(), 1) == flags.size())
        {
            //merge
            nav_msgs::OccupancyGrid map;
            
            map.info = maps.front().info;
            
            for (unsigned int i = 0; i < map.info.height * map.info.width; ++i)
            {
                map.data.push_back(-1);
                
                for (std::vector<nav_msgs::OccupancyGrid>::iterator j = maps.begin(); j != maps.end(); ++j)
                {
                    map.data[i] = std::max(map.data[i], j->data[i]);
                }
            }
            
            mapPub.publish(map);
            
            //Make not ready
            std::fill(flags.begin(), flags.end(), 0);
        }
    }
}

void PoseTracker::parseParameters(const XmlRpc::XmlRpcValue &leds)
{
	ROS_ASSERT(leds.getType() == XmlRpc::XmlRpcValue::TypeArray);

	// if parameters is good, resize the matrix to account for number of leds used to track 
	// 4 =  id + 3Dposition
	local_points_=Eigen::MatrixXd::Zero(leds.size(),4);
	Nleds_ = leds.size();

	for( int i = 0; i < Nleds_; ++i)
	{
		XmlRpc::XmlRpcValue current_led = leds[i];

		// parse ID
		ROS_ASSERT(current_led.getType() == XmlRpc::XmlRpcValue::TypeStruct);
    		if( current_led.hasMember("id") )
    		{
    			ROS_ASSERT(current_led["id"].getType() == XmlRpc::XmlRpcValue::TypeInt);
    			int id = current_led["id"];
    			local_points_(i, 0) = (double) id;
    		}
    		else
    		{
    			ROS_ERROR("No id value for the current led. Check the yaml configuration for this object");
    			return;
    		}

	    	// parse position
	    	std::vector<double> position;
	    	if( current_led.hasMember("position") )
	    	{
			for (int j = 0; j < current_led["position"].size(); ++j) 
			{
				ROS_ASSERT(current_led["position"][j].getType() == XmlRpc::XmlRpcValue::TypeDouble);
				position.push_back( current_led["position"][j] );
			}
			local_points_(i, 1) = position[0];
			local_points_(i, 2) = position[1];
			local_points_(i, 3) = position[2];
	    	}
    		else
		{
			ROS_ERROR("No double value for the position current led. Check the yaml configuration for this object, remember to use . dots to ensure double format");
			return;
		}
	}

	ROS_INFO("Succesfully parsed all LED parameters!");
	// std::cout << local_points_ << std::endl;

	return;
}

bool HmValue::ListConnections( XmlRpc::XmlRpcValue& list )
{
	if( _sendingChannel )
	{
		XmlRpcValue dict;
		dict["Alias"] = _sendingChannel->GetAlias();
		dict["Name"] = _sendingChannel->GetName();
		dict["DisplayName"] = _sendingChannel->GetDisplayName();
		if( _channel->GetDevice()->GetInterface() )
		{
			dict["XmlRpcInterfaceId"] = _channel->GetDevice()->GetInterface()->GetId();
			dict["XmlRpcInterfaceUrl"] = _channel->GetDevice()->GetInterface()->GetUrl();
			dict["XmlRpcDevice"] = _channel->GetDevice()->GetSerial();
			dict["XmlRpcChannel"] = _channel->GetSerial();
			dict["XmlRpcValue"] = GetId();
		}
		dict["Device"] = _channel->GetDevice()->GetId();
		dict["DeviceDisplayName"] = _channel->GetDevice()->GetDisplayName();
		dict["ChannelIndex"] = _channel->GetIndex();
		dict["Terminal"] = _channel->GetTerminal();
		dict["Direction"] = "Input";
		list[list.size()] = dict;
	}
	if( _receivingConnection )
	{
		std::vector<IcChannel*> receivingChannels = _receivingConnection->GetReceivingChannels();
		for( unsigned int i=0; i<receivingChannels.size(); i++ )
		{
			XmlRpcValue dict;
			dict["Alias"] = receivingChannels[i]->GetAlias();
			dict["Name"] = receivingChannels[i]->GetName();
			dict["DisplayName"] = receivingChannels[i]->GetDisplayName();
			if( _channel->GetDevice()->GetInterface() )
			{
				dict["XmlRpcInterfaceId"] = _channel->GetDevice()->GetInterface()->GetId();
				dict["XmlRpcInterfaceUrl"] = _channel->GetDevice()->GetInterface()->GetUrl();
				dict["XmlRpcDevice"] = _channel->GetDevice()->GetSerial();
				dict["XmlRpcChannel"] = _channel->GetSerial();
				dict["XmlRpcValue"] = GetId();
			}
			dict["Device"] = _channel->GetDevice()->GetId();
			dict["DeviceDisplayName"] = _channel->GetDevice()->GetDisplayName();
			dict["ChannelIndex"] = _channel->GetIndex();
			dict["Terminal"] = _channel->GetTerminal();
			dict["Direction"] = "Output";
			list[list.size()] = dict;
		}
	}
	return true;
}

bool SensorConfiguration::XmlRpcValueToEigenXd(XmlRpc::XmlRpcValue& field,
    Eigen::VectorXd *target) {

  target->resize(field.size());

  for (int k = 0; k < field.size(); k++) {
    if (field[k].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
      return false;
    }

    (*target)[k] = static_cast<double>(field[k]);
  }

  return true;
}

void TeleopCOB::getConfigurationFromParameters()
{
	//std::map<std::string,joint_module> joint_modules; //std::vector<std::string> module_names;
	if(n_.hasParam("modules"))
	{
		XmlRpc::XmlRpcValue modules;
		ROS_DEBUG("modules found ");
		n_.getParam("modules", modules);
		if(modules.getType() == XmlRpc::XmlRpcValue::TypeStruct)
		{
			ROS_DEBUG("modules are of type struct with size %d",(int)modules.size());

			for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator p=modules.begin();p!=modules.end();++p)
			{
				std::string mod_name = p->first;
				ROS_DEBUG("module name: %s",mod_name.c_str());
				XmlRpc::XmlRpcValue mod_struct = p->second;
				if(mod_struct.getType() != XmlRpc::XmlRpcValue::TypeStruct)
					ROS_WARN("invalid module, name: %s",mod_name.c_str());
				// search for joint_name parameter in current module struct to determine which type of module
				// only joint mods or wheel mods supported
				// which mens that is no joint names are found, then the module is a wheel module
				// TODO replace with build in find, but could not get it to work
				if(!assign_joint_module(mod_name, mod_struct))
				{
					// add wheel module struct
					ROS_DEBUG("wheel module found");
					assign_base_module(mod_struct);
				}
			}
		}
	}
}

void FootstepMarker::readPoseParam(ros::NodeHandle& pnh, const std::string param,
                                   tf::Transform& offset) {
  XmlRpc::XmlRpcValue v;
  geometry_msgs::Pose pose;
  if (pnh.hasParam(param)) {
    pnh.param(param, v, v);
    // check if v is 7 length Array
    if (v.getType() == XmlRpc::XmlRpcValue::TypeArray &&
        v.size() == 7) {
      // safe parameter access by getXMLDoubleValue
      pose.position.x = getXMLDoubleValue(v[0]);
      pose.position.y = getXMLDoubleValue(v[1]);
      pose.position.z = getXMLDoubleValue(v[2]);
      pose.orientation.x = getXMLDoubleValue(v[3]);
      pose.orientation.y = getXMLDoubleValue(v[4]);
      pose.orientation.z = getXMLDoubleValue(v[5]);
      pose.orientation.w = getXMLDoubleValue(v[6]);
      // converst the message as following: msg -> eigen -> tf
      //void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e);
      Eigen::Affine3d e;
      tf::poseMsgToEigen(pose, e); // msg -> eigen
      tf::transformEigenToTF(e, offset); // eigen -> tf
    }
    else {
      ROS_ERROR_STREAM(param << " is malformed, which should be 7 length array");
    }
  }
  else {
    ROS_WARN_STREAM("there is no parameter on " << param);
  }
}

bool getNodes(V_string& nodes)
{
  XmlRpc::XmlRpcValue args, result, payload;
  args[0] = this_node::getName();

  if (!execute("getSystemState", args, result, payload, true))
  {
    return false;
  }

  S_string node_set;
  for (int i = 0; i < payload.size(); ++i)
  {
    for (int j = 0; j < payload[i].size(); ++j)
    {
      XmlRpc::XmlRpcValue val = payload[i][j][1];
      for (int k = 0; k < val.size(); ++k)
      {
        std::string name = payload[i][j][1][k];
        node_set.insert(name);
      }
    }
  }

  nodes.insert(nodes.end(), node_set.begin(), node_set.end());

  return true;
}

bool FTParamsInternal::getDoubleArray(XmlRpc::XmlRpcValue params, const char* name, double *results, unsigned len)
{
  if(!params.hasMember(name))
  {
    ROS_ERROR("Expected ft_param to have '%s' element", name);
    return false;
  }

  XmlRpc::XmlRpcValue values = params[name];
  if (values.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Expected FT param '%s' to be list type", name);
    return false;
  }
  if (values.size() != int(len))
  {
    ROS_ERROR("Expected FT param '%s' to have %d elements", name, len);
    return false;
  }
  for (unsigned i=0; i<len; ++i)
  {
    if (values[i].getType() != XmlRpc::XmlRpcValue::TypeDouble)
    {
      ROS_ERROR("Expected FT param %s[%d] to be floating point type", name, i);
      return false;
    } else {
      results[i] = static_cast<double>(values[i]);
    }
  }

  return true;
}

/*generateConfigVector stores the pinging sensors, to which pinging sensor is a listening
 * sensor bound to in a cycle and which sensors are not used at all.
 * In the output, the pinging sensors are represented by PINGING_SENSOR, location of
 * each listening sensor holds the address of its corresponding pinging sensor
 * and the sensors not used at all are represented by SENSOR_NOT_USED.
 */
std::vector <std::vector<int> > generateConfigVector(XmlRpc::XmlRpcValue config_list)
{
	std::vector<std::vector<int> > config;
	XmlRpc::XmlRpcValue current_cycle;
	ROS_ASSERT(config_list.getType() == XmlRpc::XmlRpcValue::TypeArray);

	for(int i = 0; i < config_list.size(); i++)
	{
		ROS_ASSERT(config_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct);
		std::vector<int> cycle_vector; //For holding configuration for each cycle.
		cycle_vector.resize(MAX_SENSORS);
		for (int j=0; j<MAX_SENSORS; j++)
			cycle_vector[j]=SENSOR_NOT_USED;

		//better to keep it dynamic
		for (int j = 0; j<MAX_SENSORS; j++){
			char str_index[3] = {'\0','\0', '\0'}; // for storing int to hex conv. 
			sprintf(str_index,"%d", j); // Unsure if yamlcpp parser reads hexadecimal values
			if(config_list[i].hasMember(str_index))
			{
				cycle_vector[j]=PINGING_SENSOR; //Set -1 for pinging sensor
				current_cycle = (config_list[i]).operator[](str_index);
				ROS_ASSERT(current_cycle.getType() == XmlRpc::XmlRpcValue::TypeArray);
				ROS_ASSERT(current_cycle.size()>0);
				//Assign address of pinging sensor to each listening sensor
				for (int k = 0; k<current_cycle.size(); k++){
					cycle_vector[static_cast<int>(current_cycle[k])]=((static_cast<int>(current_cycle[k])!=j)?j:PINGING_AND_LISTENING_SENSOR);
				}
			}
		}
		config.push_back(cycle_vector);
	}
	return config;
}

bool TestCollisionWorld::readDoubleArray(XmlRpc::XmlRpcValue& list, std::vector<double>& array)
{
    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
        ROS_ERROR("Parameter needs to be an *array* of doubles");
        return false;
    }

    array.clear();
    for (int32_t i = 0; i < list.size(); ++i)
    {
        if (list[i].getType() != XmlRpc::XmlRpcValue::TypeDouble &&
                list[i].getType() != XmlRpc::XmlRpcValue::TypeInt)
        {
            ROS_ERROR("Parameter needs to be an array of *doubles*");
            return false;
        }
        double value=0.0;
        if (list[i].getType() == XmlRpc::XmlRpcValue::TypeInt)
            value = static_cast<double>(static_cast<int>(list[i]));
        else value = static_cast<double>(list[i]);
        array.push_back(value);
    }
    return true;
}

template <typename T> bool read_vector(ros::NodeHandle &n_, const std::string &key, std::vector<T> & res){
    XmlRpc::XmlRpcValue namesXmlRpc;
    if (!n_.hasParam(key))
    {
	return false;
    }

    n_.getParam(key, namesXmlRpc);
    /// Resize and assign of values to the vector
    res.resize(namesXmlRpc.size());
    for (int i = 0; i < namesXmlRpc.size(); i++)
    {
    	res[i] = (T)namesXmlRpc[i];
    }
    return true;
}

void BeaconKFNode::getCovarianceMatrix(std::string param_name, MatrixWrapper::SymmetricMatrix& m)
{
    ros::NodeHandle private_nh("~");

    XmlRpc::XmlRpcValue values;
    private_nh.getParam(param_name, values);
    if( values.getType() != XmlRpc::XmlRpcValue::TypeArray )
    {
        ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, not an array", param_name.c_str());
        return;
    }
    if( values.size() < 6 )
    {
        ROS_ERROR("BEACON LOCALIZER Unable to read covariance %s, array too short: %d", param_name.c_str(), values.size());
        return;
    }
    int i=0;
    for (uint32_t row = 1; row <= m.rows(); ++row)
    {
        for (uint32_t column = row; column <= m.columns(); ++column)
        {
            double x;
            if(i>=values.size())
            {
                ROS_ERROR("BEACON LOCALIZER Need at least 6 values, have %d", i);
                return;
            }
            XmlRpc::XmlRpcValue value=values[i++];
            if( value.getType() == XmlRpc::XmlRpcValue::TypeInt )
            {
                x=int(value);
            }
            else if(value.getType() == XmlRpc::XmlRpcValue::TypeDouble )
            {
                x=double(value);
            }
            else
            {
                std::string vstr = value;
                ROS_ERROR("BEACON LOCALIZER Unable to read covariance matrix %s, value at %d is not a number: %s",
                        param_name.c_str(), i, vstr.c_str());
                return;
            }
            m(row, column) = x;
        }
    }
}