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

bool DepthImageOccupancyMapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  try
  {
    sensor_type_ = (std::string) params["sensor_type"];
    if (params.hasMember("image_topic"))
      image_topic_ = (std::string) params["image_topic"];
    if (params.hasMember("queue_size"))
      queue_size_ = (int)params["queue_size"];
    
    readXmlParam(params, "near_clipping_plane_distance", &near_clipping_plane_distance_);
    readXmlParam(params, "far_clipping_plane_distance", &far_clipping_plane_distance_);
    readXmlParam(params, "shadow_threshold", &shadow_threshold_);
    readXmlParam(params, "padding_scale", &padding_scale_);
    readXmlParam(params, "padding_offset", &padding_offset_);
    readXmlParam(params, "skip_vertical_pixels", &skip_vertical_pixels_);
    readXmlParam(params, "skip_horizontal_pixels", &skip_horizontal_pixels_);
  }
  catch (XmlRpc::XmlRpcException &ex)
  {
    ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
    return false;
  }
  
  return true;
}

 bool PointCloudMoveitFilter::setParams(XmlRpc::XmlRpcValue &params)
 {
   try
   {
     if (!params.hasMember("point_cloud_topic"))
       return false;
     point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);
     
     readXmlParam(params, "max_range", &max_range_);
     readXmlParam(params, "padding_offset", &padding_);
     readXmlParam(params, "padding_scale", &scale_);
     readXmlParam(params, "point_subsample", &point_subsample_);
     if (!params.hasMember("filtered_cloud_topic")) {
       ROS_ERROR("filtered_cloud_topic is required");
       return false;
     }
     else {
       filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
     }
     if (params.hasMember("filtered_cloud_use_color")) {
       use_color_ = (bool)params["filtered_cloud_use_color"];
     }
     if (params.hasMember("filtered_cloud_keep_organized")) {
       keep_organized_ = (bool)params["filtered_cloud_keep_organized"];
     }
   }
   catch (XmlRpc::XmlRpcException& ex)
   {
     ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
     return false;
   }
   
   return true;
 }

Path::Path() {
	ros::NodeHandle nh;
	XmlRpc::XmlRpcValue pathCfg;
	nh.getParam("/path", pathCfg);
	try {
		int i = 0;
		do {
			char pointName[256];
			sprintf(pointName, "point%d", i);
			if (!pathCfg.hasMember(pointName))
				break;

			XmlRpc::XmlRpcValue pointCfg = pathCfg[std::string(pointName)];
			geometry_msgs::Point p;
			if (!(pointCfg.hasMember("x") && pointCfg.hasMember("y")))
				continue;

			p.x = FLOAT_PARAM(pointCfg["x"]);
			p.y = FLOAT_PARAM(pointCfg["y"]);
			p.z = 0;

			points.push_back(p);
		} while ((++i) != 0);
	} catch (XmlRpc::XmlRpcException& e) {
		ROS_ERROR("Unable to parse path parameter. (%s)", e.getMessage().c_str());
	}
}

PulseMotorController::PulseMotorController(
    XmlRpc::XmlRpcValue &pulse_motor_info)
    : motor_regs_(NULL), start_count_(0), last_target_(0) {
    ROS_ASSERT(pulse_motor_info.getType() == XmlRpc::XmlRpcValue::TypeStruct);
    ROS_ASSERT(pulse_motor_info.hasMember("name"));
    ROS_ASSERT(pulse_motor_info.hasMember("dev_filename"));
    ROS_ASSERT(pulse_motor_info.hasMember("gpio_filename"));
    ROS_ASSERT(pulse_motor_info.hasMember("to_meter_fraction"));
    ROS_ASSERT(pulse_motor_info.hasMember("speed"));

    // Open device files
    name_ = (string)pulse_motor_info["name"];
    string dev_file = pulse_motor_info["dev_filename"];
    legal_checker_ =
        SwitchGPIOPtr(new SwitchGPIO(pulse_motor_info["gpio_filename"]));
    dev_fd_ = open(dev_file.c_str(), O_RDWR, 0);
    if (dev_fd_ < 0) {
        ROS_ERROR("Failed to open file %s for motor %s", dev_file.c_str(),
                  name_.c_str());
        return;
    }
    ROS_ASSERT(sizeof(unsigned) == 4);
    motor_regs_ =
        (unsigned *)mmap(NULL, sizeof(unsigned) * 4, PROT_READ | PROT_WRITE,
                         MAP_SHARED, dev_fd_, 0);
    if ((long long)motor_regs_ == -1) {
        close(dev_fd_);
        ROS_ERROR("Failed to create memory map for motor %s", name_.c_str());
        motor_regs_ = NULL;
        return;
    }

    // Check initial position
    printf("fuck 1\n");
    ROS_INFO("Motor %s suceessfully mapped file %s", name_.c_str(),
             dev_file.c_str());
    can_move_ = !legal_checker_->Get();
    printf("fuck 2\n");

    // Setup
    to_meter_fraction_ = pulse_motor_info["to_meter_fraction"];
    printf("fuck 3\n");
    ROS_INFO("get to_meter_fraction");
    if (!can_move_) ROS_WARN("Motor %s not at initial position", name_.c_str());
    speed_ = (unsigned)((int)pulse_motor_info["speed"]);
    printf("fuck 4\n");
    motor_regs_[kCTRL] = CTRL_DIR & ~CTRL_ENABLE;
    motor_regs_[kVEL] = speed_;
    start_count_ = motor_regs_[kFEEDBACK];
    printf("fuck 5\n");
    ROS_INFO("Pulse motor %s starts with count %u", name_.c_str(), start_count_);
}

Servo::Servo(XmlRpc::XmlRpcValue& servo_info)
      : channel_(-1)
      , type_(RCSERVO_SERVO_DEFAULT)
      , joint_name_("")
      , trim_pwm_(0)
      , max_pwm_(2300)
      , min_pwm_(700)     
      , max_rot_(HALFPI)
      , min_rot_(-HALFPI)
      , bus_(PWM)
{
  if (servo_info.hasMember("joint_name"))
    joint_name_ = (std::string)servo_info["joint_name"];
  
	if (servo_info.hasMember("channel"))
  	channel_ = servo_info["channel"];

  if (servo_info.hasMember("type"))
  {
    std::string type_string = servo_info["type"];
    if (type_string == "RCSERVO_KONDO_KRS78X")
      type_ = RCSERVO_KONDO_KRS78X;
    else if (type_string == "RCSERVO_KONDO_KRS788")
      type_ = RCSERVO_KONDO_KRS788;
    else if (type_string == "RCSERVO_KONDO_KRS4014")
      type_ = RCSERVO_KONDO_KRS4014;      
    else if (type_string == "RCSERVO_HITEC_HSR8498")
      type_ = RCSERVO_HITEC_HSR8498;
    else if (type_string == "KONDO_ROS3030")
    	type_ = KONDO_ROS3030;
    else
      type_ = RCSERVO_SERVO_DEFAULT; 
  }   

  if (servo_info.hasMember("bus")) 
  {
    std::string bus_string = servo_info["bus"];
    if (bus_string == "COM4")
      bus_ = COM4;
  }

  if (servo_info.hasMember("min_rotation"))
    min_rot_ = (double)servo_info["min_rotation"];
  if (servo_info.hasMember("max_rotation"))
    max_rot_ = (double)servo_info["max_rotation"];  
    
  if (servo_info.hasMember("min_pwm"))
    min_pwm_ = (int)servo_info["min_pwm"];
  if (servo_info.hasMember("max_pwm"))
    max_pwm_ = (int)servo_info["max_pwm"];    
    
  if (servo_info.hasMember("trim"))
    trim_pwm_ = servo_info["trim"];  
}

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;
}

void CServo::setParams(XmlRpc::XmlRpcValue params)
{
    if(params.hasMember("id"))
    {
      id_=params["id"];
    }
    if(params.hasMember("max_angle_degrees"))
    {
      max_angle_=(double)radians(params["max_angle_degrees"]);
    }
    if(params.hasMember("min_angle_degrees"))
    {
      min_angle_=(double)radians(params["min_angle_degrees"]);
    }
    if(params.hasMember("max_angle_radians"))
    {
      max_angle_=(double)(params["max_angle_radians"]);
    }
    if(params.hasMember("min_angle_radians"))
    {
      min_angle_=(double)(params["min_angle_radians"]);
    }
    if(params.hasMember("max_speed"))
    {
      max_speed_=(double)params["max_speed"];
    }
    if(params.hasMember("range"))
    {
      range_=(double)radians(params["range"]);
    }
    if(params.hasMember("ticks"))
    {
      ticks_=params["ticks"];
    }
    if(params.hasMember("neutral"))
    {
      neutral_=params["neutral"];
    }
    if(params.hasMember("invert"))
    {
      invert_=params["invert"];
    }
    recalculateAngleParams();
}

void OccupancyMapUpdater::readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, double *value)
{
  if (params.hasMember(param_name))
  {
    if (params[param_name].getType() == XmlRpc::XmlRpcValue::TypeInt)
      *value = (int) params[param_name];
    else
      *value = (double) params[param_name];
  }
}

bool getParam(XmlRpc::XmlRpcValue& config, const std::string& key, double& value)
{
  if (!config.hasMember(key))
  {
    return false;
  }
  XmlRpc::XmlRpcValue param = config[key];
  if (param.getType() != XmlRpc::XmlRpcValue::TypeDouble)
  {
    return false;
  }
  value = param;
  return true;
}

bool PublishedRigidBody::validateParam(XmlRpc::XmlRpcValue &config_node, const std::string &name)
{
  if (!config_node.hasMember(name))
  {
    return false;
  }

  if (config_node[name].getType() != XmlRpc::XmlRpcValue::TypeString)
  {
    return false;
  }

  return true;
}

bool PointCloudOctomapUpdater::setParams(XmlRpc::XmlRpcValue &params)
{
  try
  {
    if (!params.hasMember("point_cloud_topic"))
      return false;
    point_cloud_topic_ = static_cast<const std::string&>(params["point_cloud_topic"]);

    readXmlParam(params, "max_range", &max_range_);
    readXmlParam(params, "padding_offset", &padding_);
    readXmlParam(params, "padding_scale", &scale_);
    readXmlParam(params, "point_subsample", &point_subsample_);
    if (params.hasMember("filtered_cloud_topic"))
      filtered_cloud_topic_ = static_cast<const std::string&>(params["filtered_cloud_topic"]);
  }
  catch (XmlRpc::XmlRpcException &ex)
  {
    ROS_ERROR("XmlRpc Exception: %s", ex.getMessage().c_str());
    return false;
  }

  return true;
}

bool VisualizationBase::readParameters(XmlRpc::XmlRpcValue& config)
{
  if (config.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
    ROS_ERROR("A filter configuration must be a map with fields name, type, and params.");
    return false;
  }

  // Check to see if we have parameters in our list.
  if (config.hasMember("params")) {
    XmlRpc::XmlRpcValue params = config["params"];
    if (params.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
      ROS_ERROR("Params must be a map.");
      return false;
    } else {
      for (XmlRpc::XmlRpcValue::iterator it = params.begin(); it != params.end(); ++it) {
        ROS_DEBUG("Loading param %s\n", it->first.c_str());
        parameters_[it->first] = it->second;
      }
    }
  }

  return true;
}

static void readXmlParam(XmlRpc::XmlRpcValue &params, const std::string &param_name, unsigned int *value)
{
  if (params.hasMember(param_name))
    *value = (int) params[param_name];
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "standalone_complexed_nodelet");
  ros::NodeHandle private_nh("~");
  ros::NodeHandle nh;
  nodelet::Loader manager(false); // Don't bring up the manager ROS API
  nodelet::V_string my_argv;  
  std::vector<std::string> candidate_root;
  candidate_root.push_back("nodelets");
  int candidate_num;
  private_nh.param("candidate_num", candidate_num, 100);
  for (size_t i = 0; i < candidate_num; i++) {
    candidate_root.push_back((boost::format("nodelets_%lu") % i).str());
  }
  for (size_t i_candidate = 0; i_candidate < candidate_root.size(); i_candidate++) {
    std::string root_name = candidate_root[i_candidate];
    if (private_nh.hasParam(root_name)) {
      XmlRpc::XmlRpcValue nodelets_values;
      private_nh.param(root_name, nodelets_values, nodelets_values);
      if (nodelets_values.getType() == XmlRpc::XmlRpcValue::TypeArray) {
        for (size_t i_nodelet = 0; i_nodelet < nodelets_values.size(); i_nodelet++) {
          ROS_INFO("i_nodelet %lu", i_nodelet);
          XmlRpc::XmlRpcValue onenodelet_param = nodelets_values[i_nodelet];
          if (onenodelet_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
            std::string name, type;
            nodelet::M_string remappings;
            if (onenodelet_param.hasMember("if") && !(bool)onenodelet_param["if"]) {
              continue;
            }
            if (onenodelet_param.hasMember("unless") && (bool)onenodelet_param["unless"]) {
              continue;
            }
            if (onenodelet_param.hasMember("name")) {
              name = nh.resolveName((std::string)onenodelet_param["name"]);
            }
            else {
              ROS_FATAL("element ~nodelets should have name field");
              return 1;
            }
            if (onenodelet_param.hasMember("type")) {
              type = (std::string)onenodelet_param["type"];
            }
            else {
              ROS_FATAL("element ~nodelets should have type field");
              return 1;
            }
            if (onenodelet_param.hasMember("remappings")) {
              XmlRpc::XmlRpcValue remappings_params
                = onenodelet_param["remappings"];
              if (remappings_params.getType() == XmlRpc::XmlRpcValue::TypeArray) {
                for (size_t remappings_i = 0; remappings_i < remappings_params.size(); remappings_i++) {
                  XmlRpc::XmlRpcValue remapping_element_param = remappings_params[remappings_i];
                  if (remapping_element_param.getType() == XmlRpc::XmlRpcValue::TypeStruct) {
                    if (remapping_element_param.hasMember("from") && remapping_element_param.hasMember("to")) {
                      std::string from = (std::string)remapping_element_param["from"];
                      std::string to = (std::string)remapping_element_param["to"];
                      if (from.size() > 0 && from[0] == '~') {
                        ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
                        from = nodelet_private_nh.resolveName(from.substr(1, from.size() - 1));
                      }
                      else {
                        ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
                        from = nodelet_nh.resolveName(from);
                      }
                      if (to.size() > 0 && to[0] == '~') {
                        ros::NodeHandle nodelet_private_nh = ros::NodeHandle(name);
                        to = nodelet_private_nh.resolveName(to.substr(1, to.size() - 1));
                      }
                      else {
                        ros::NodeHandle nodelet_nh = ros::NodeHandle(parentName(name));
                        to = nodelet_nh.resolveName(to);
                      }
                      ROS_INFO("remapping: %s => %s", from.c_str(), to.c_str());
                      remappings[from] = to;
                    }
                    else {
                      ROS_FATAL("remappings parameter requires from and to fields");
                      return 1;
                    }
                  }
                  else {
                    ROS_FATAL("remappings should be an array");
                    return 1;
                  }
                }
              }
              else {
                ROS_FATAL("remappings should be an array");
                return 1;
              }
            }
            if (onenodelet_param.hasMember("params")) {
              if (onenodelet_param["params"].getType() != XmlRpc::XmlRpcValue::TypeArray) {
                ROS_FATAL("params parameter must be an array");
                return 1;
              }
              XmlRpc::XmlRpcValue values;
              for (int params_i = 0; params_i < onenodelet_param["params"].size(); ++params_i) {
                XmlRpc::XmlRpcValue oneparam = onenodelet_param["params"][params_i];
                if (!(oneparam.getType() == XmlRpc::XmlRpcValue::TypeStruct &&
//.........这里部分代码省略.........

SolverConfiguration::SolverConfiguration(XmlRpc::XmlRpcValue& conf) {

  if (conf.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
    ROS_FATAL("solver: bad configuration, expecting structure.");
    ROS_BREAK();
  }

  if (!conf.hasMember("base_link_frame_id")
      || conf["base_link_frame_id"].getType()
          != XmlRpc::XmlRpcValue::TypeString) {

    std::cerr << conf["type"].getType() << std::endl;
    ROS_FATAL("solver: 'base_link_frame_id' field undefined or not a string");
    ROS_BREAK();
  }

  base_link_frame_id_ = static_cast<std::string &>(conf["base_link_frame_id"]);

  if (!conf.hasMember("global_frame_id")
      || conf["global_frame_id"].getType() != XmlRpc::XmlRpcValue::TypeString) {
    ROS_FATAL("solver: 'global_frame' field undefined or not a string");
    ROS_BREAK();
  }

  global_frame_ = static_cast<std::string &>(conf["global_frame_id"]);

  if (!conf.hasMember("pose_window_length")
      || conf["pose_window_length"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
    ROS_FATAL("solver: 'pose_window_length' field undefined or not a double");
    ROS_BREAK();
  }

  pose_window_length_ = static_cast<double>(conf["pose_window_length"]);

  if (pose_window_length_ < 0) {
    ROS_FATAL("solver: 'pose_window_length' field must be greater than zero");
    ROS_BREAK();
  }

  if (!conf.hasMember("dead_reckoning")
      || conf["dead_reckoning"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
    ROS_FATAL("solver: 'dead_reckoning' field undefined or not a boolean");
    ROS_BREAK();
  }

  dead_reckoning_ = static_cast<bool>(conf["dead_reckoning"]);

  if (!conf.hasMember("frequency")
      || conf["frequency"].getType() != XmlRpc::XmlRpcValue::TypeDouble) {
    ROS_FATAL("solver: 'frequency' field undefined or not a double");
    ROS_BREAK();
  }

  frequency_ = static_cast<double>(conf["frequency"]);

  if (!conf.hasMember("n_gauss_newton_iterations")
      || conf["n_gauss_newton_iterations"].getType()
          != XmlRpc::XmlRpcValue::TypeInt) {
    ROS_FATAL(
        "solver: 'n_gauss_newton_iterations' field undefined or not an integer");
    ROS_BREAK();
  }

  n_gauss_newton_iterations_ =
      static_cast<int>(conf["n_gauss_newton_iterations"]);

  if (!conf.hasMember("low_level_logging")
      || conf["low_level_logging"].getType() != XmlRpc::XmlRpcValue::TypeBoolean) {
    ROS_FATAL("solver: 'low_level_logging' field undefined or not a boolean");
    ROS_BREAK();
  }

  low_level_logging_ = static_cast<bool>(conf["low_level_logging"]);
}