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

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

/**
 * Tries to read joint module configurations from XmlRpcValue object.
 * If the module is a joint module, it contains a joint name array.
 * A typical joint module has the following configuration structure:
 * struct {
 * 	  joint_names: ['head_pan_joint','head_tilt_joint'],
 * 	  joint_step: 0.075
 * }
 * @param mod_struct configuration object struct
 * @return true if the configuration object hols a joint module config, else false
 */
bool TeleopCOB::assign_joint_module(std::string mod_name, XmlRpc::XmlRpcValue mod_struct)
{
	// 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
	bool is_joint_module = false;
	joint_module tempModule;
	for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
	{
		std::string par_name = ps->first;
		ROS_DEBUG("par name: %s",par_name.c_str());

		if(par_name.compare("joint_names")==0)
		{
			ROS_DEBUG("joint names found");
			XmlRpc::XmlRpcValue joint_names = ps->second;

			ROS_ASSERT(joint_names.getType() == XmlRpc::XmlRpcValue::TypeArray);
			ROS_DEBUG("joint_names.size: %d \n", joint_names.size());
			for(int i=0;i<joint_names.size();i++)
			{
				ROS_ASSERT(joint_names[i].getType() == XmlRpc::XmlRpcValue::TypeString);
				std::string s((std::string)joint_names[i]);
				ROS_DEBUG("joint_name found = %s",s.c_str());
				tempModule.joint_names.push_back(s);
			}
			// set size of other vectors according to the joint name vector
			tempModule.req_joint_pos_.resize(joint_names.size());
			tempModule.req_joint_vel_.resize(joint_names.size());

			is_joint_module = true;
			//break; // no need to continue searching if joint names are found
		}else if(par_name.compare("joint_step")==0){
			ROS_DEBUG("joint steps found");
			XmlRpc::XmlRpcValue joint_steps = ps->second;

			ROS_ASSERT(joint_steps.getType() == XmlRpc::XmlRpcValue::TypeArray);
			ROS_DEBUG("joint_steps.size: %d \n", joint_steps.size());
			for(int i=0;i<joint_steps.size();i++)
			{
				ROS_ASSERT(joint_steps[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
				double step((double)joint_steps[i]);
				ROS_DEBUG("joint_step found = %f",step);
				tempModule.steps.push_back(step);
			}
		}
	}
	if(is_joint_module)
	{
		// assign publisher
		tempModule.module_publisher_ = n_.advertise<trajectory_msgs::JointTrajectory>(("/"+mod_name+"_controller/command"),1);
		tempModule.module_publisher_brics_ = n_.advertise<brics_actuator::JointVelocities>(("/"+mod_name+"_controller/command_vel"),1);
		// store joint module in collection
		ROS_DEBUG("head module stored");
		joint_modules_.insert(std::pair<std::string,joint_module>(mod_name,tempModule));
	}
	return is_joint_module;
}

void EmotionGenerator::getEmotionDesireRelation(std::string desireType)
{
	XmlRpc::XmlRpcValue emotionParam;
	std::string namespaceNode = n_->getNamespace().substr(1,n_->getNamespace().length()-1);
	std::string paramServerDesire = namespaceNode + "/" + nodeName_ + "/" + desireType;
	n_->getParam(paramServerDesire, emotionParam);

	//	ROS_INFO("Emo gen - param emotion desire relation :%s,%s,%s = %s", n_->getNamespace().c_str() ,
	//									   nodeName_.c_str(),
	//									   desireType.c_str(),
	//									   paramServerDesire.c_str() );

	std::map<std::string,double> mapEmotion;

	ROS_INFO_COND(emotionParam.begin() != emotionParam.end() , "new desire %s :",desireType.c_str());
	for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator it = emotionParam.begin(); it != emotionParam.end(); it++)
	{
		double emotionFactor = 0;
		if((*it).second.getType() == XmlRpc::XmlRpcValue::TypeInt)
		{
			int value = static_cast<int>((*it).second);
			ROS_INFO(" %s %i",(*it).first.c_str(), value);
			emotionFactor = (double)value;

		}
		else if ((*it).second.getType() == XmlRpc::XmlRpcValue::TypeDouble)
		{
			emotionFactor = static_cast<double>((*it).second);
			ROS_INFO(" %s %5.2f",(*it).first.c_str(), emotionFactor);
		}

		mapEmotion[(*it).first] = emotionFactor;

		if(emotionIntensities.count((*it).first) == 0)
		{
			//initialize this emotion
			emotionIntensities[(*it).first] = 0;
		}
	}

	if(mapEmotion.size() > 0)
		emotionMatrix[desireType] = mapEmotion;
}

/**
 * Tries to read base module configurations from XmlRpcValue object.
 * A base module is supposed to contain vectors 3 element vectors (x,y,th)
 * with max acceleration and velocity. Example:
 * struct {
 * 	   max_velocity: [0.3, 0.2, 0.3],
 * 	  max_acceleration: [0.5, 0.5, 0.7]
 * }
 * @param mod_struct configuration object struct
 * @return true no check is currently performed TODO check
 */
bool TeleopCOB::assign_base_module(XmlRpc::XmlRpcValue mod_struct)
{
	for(std::map<std::string,XmlRpc::XmlRpcValue>::iterator ps=mod_struct.begin();ps!=mod_struct.end();++ps)
	{
		std::string par_name = ps->first;
		ROS_DEBUG("par name: %s",par_name.c_str());

		if(par_name.compare("max_velocity")==0)
		{
			ROS_DEBUG("max vel found");
			XmlRpc::XmlRpcValue max_vel = ps->second;

			ROS_ASSERT(max_vel.getType() == XmlRpc::XmlRpcValue::TypeArray);
			if(max_vel.size()!=3){ROS_WARN("invalid base parameter size");}
			ROS_DEBUG("max_vel.size: %d \n", max_vel.size());
			for(int i=0;i<max_vel.size();i++)
			{
				ROS_ASSERT(max_vel[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
				double val = (double)max_vel[i];
				ROS_DEBUG("max vel value = %f",val);
				base_module_.max_vel_.push_back(val);
			}
		}
		else if(par_name.compare("max_acceleration")==0)
		{
			ROS_DEBUG("max acc found");
			XmlRpc::XmlRpcValue max_acc = ps->second;

			ROS_ASSERT(max_acc.getType() == XmlRpc::XmlRpcValue::TypeArray);
			if(max_acc.size()!=3){ROS_DEBUG("invalid base parameter size");}
			ROS_DEBUG("max_acc.size: %d \n", max_acc.size());
			for(int i=0;i<max_acc.size();i++)
			{
				ROS_ASSERT(max_acc[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
				double val = (double)max_acc[i];
				ROS_DEBUG("max acc value = %f", val);
				base_module_.max_acc_.push_back(val);
			}
		}
		else
		{
			ROS_WARN("unsupported base module parameter read");
		}
	}
	// make all the vectors the same length
	// the vector size is not completely safe since only warning is
	// issued if max value arrays has the wrong length
	base_module_.req_vel_.resize(base_module_.max_acc_.size());
	base_module_.vel_old_.resize(base_module_.max_acc_.size());
	base_module_.base_publisher_ = n_.advertise<geometry_msgs::Twist>("/base_controller/command",1);
	ROS_DEBUG("base module stored");
	has_base_module_ = true;
	return true;
}

hsvColorRange::hsvColorRange(XmlRpc::XmlRpcValue _params)
{
    ROS_ASSERT(_params.getType()==XmlRpc::XmlRpcValue::TypeStruct);

    for (XmlRpc::XmlRpcValue::iterator i=_params.begin(); i!=_params.end(); ++i)
    {
        ROS_ASSERT(i->second.getType()==XmlRpc::XmlRpcValue::TypeArray);
        for(int j=0; j<i->second.size(); ++j)
        {
            ROS_ASSERT(i->second[j].getType()==XmlRpc::XmlRpcValue::TypeInt);
        }
        // printf("%s %i %i\n", i->first.c_str(), static_cast<int>(i->second[0]), static_cast<int>(i->second[1]));
        if (i->first == "H") H=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
        if (i->first == "S") S=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
        if (i->first == "V") V=colorRange(static_cast<int>(i->second[0]),static_cast<int>(i->second[1]));
    }
}

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

void ServoInterface::loadServoCommandPriorities()
{
  ros::NodeHandle nhPvt = getPrivateNodeHandle();
  XmlRpc::XmlRpcValue v;
  nhPvt.param("servoCommandProirities", v, v);
  std::map<std::string, XmlRpc::XmlRpcValue>::iterator mapIt;
  for(mapIt = v.begin(); mapIt != v.end(); mapIt++)
  {
    if(mapIt->second.getType() == XmlRpc::XmlRpcValue::TypeInt)
    {
      //add entry in priority queue and command map
      priorityEntry toAdd;
      toAdd.id = mapIt->first;
      toAdd.priority = static_cast<int>(mapIt->second);
      m_servoCommandPriorities.push_back(toAdd);
      m_servoCommandMsgs[mapIt->first] = autorally_msgs::servoMSG();

    } else
    {
      NODELET_ERROR("ServoInterface: XmlRpc servo command priorities formatted incorrectly");
    }
  }
  std::sort(m_servoCommandPriorities.begin(),
            m_servoCommandPriorities.end(),
            priorityComparator());

  std::vector<priorityEntry>::const_iterator vecIt;
  for(vecIt = m_servoCommandPriorities.begin();
      vecIt != m_servoCommandPriorities.end();
      vecIt++)
  {
    NODELET_INFO_STREAM("ServoInterface: ServoCommand ID:Priorities:" << vecIt->id << ":" << vecIt->priority);
  }
  NODELET_INFO_STREAM("ServoInterface: Loaded " <<
                      m_servoCommandPriorities.size() << " servo commanders");
}

/**
 * \brief Load the configuration from file.
 * @param configurationPrefix The entry node inside the configuration YAML file.
 * @return The complete image provider configuration.
 */
ImageProviderConfiguration::ConstPtr getImageProviderConfiguration () {
    ImageProviderConfiguration::Ptr configuration = boost::make_shared<ImageProviderConfiguration>();
    // private parameters
    ros::NodeHandle privateNodeHandle ("~");
    std::string configurationPrefix;
    privateNodeHandle.param("cfgprefix", configurationPrefix, std::string("configuration"));
    privateNodeHandle.param("startpos", configuration->startPos, 0);
    privateNodeHandle.param("length", configuration->length, -1);
    privateNodeHandle.param("rate", configuration->rate, 1);
    privateNodeHandle.param("loop", configuration->loop, false);
    privateNodeHandle.param("topicprefix", configuration->topicPrefix, std::string("cameras"));
    ros::NodeHandle configNodeHandle (privateNodeHandle, configurationPrefix);
    // configuration parameters (supposed to be loaded via YAML file)
    if (!configNodeHandle.getParam("srcdir", configuration->sourceDir)) {
        configuration->sourceDir = ".";
    }
    if (!configNodeHandle.getParam("filepattern", configuration->filePattern)) {
        throw ImageProviderException("Parameter 'filepattern' not found");
    }
    try {
        XmlRpc::XmlRpcValue cameraNames;
        if (configNodeHandle.getParam("cameras", cameraNames)) {
            std::map<std::string, XmlRpc::XmlRpcValue>::iterator nameIterator;
            for (nameIterator = cameraNames.begin(); nameIterator != cameraNames.end(); ++nameIterator) {
                std::string cameraName = nameIterator->first;
                ROS_INFO_STREAM("Reading camera configuration for camera: " << cameraName);
                CameraConfigEntry::ConstPtr cameraConfig = getCameraConfigurationEntry(configNodeHandle, cameraName);
                configuration->cameraConfigs.push_back(cameraConfig);
            }
        }
    }
    catch (XmlRpc::XmlRpcException& e) {
        throw ImageProviderException(std::string("Failed to retrieve camera names: ") + std::string(e.getMessage()));
    }
    return configuration;
}

YAML::Node XmlToYaml( XmlRpc::XmlRpcValue& xml )
{
	YAML::Node yaml;
	
	if( xml.getType() != XmlRpc::XmlRpcValue::TypeStruct ) { return yaml; }
	
	XmlRpc::XmlRpcValue::iterator iter;
	for( iter = xml.begin(); iter != xml.end(); iter++ )
	{
		std::string name = iter->first;
		XmlRpc::XmlRpcValue payload = iter->second;
		if( payload.getType() == XmlRpc::XmlRpcValue::TypeStruct )
		{
			yaml[name] = XmlToYaml( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeArray )
		{
			if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeBoolean ) )
			{
				std::vector<bool> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<bool>( payload[i]) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeInt ) )
			{
				std::vector<int> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<int>( payload[i]) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeDouble ) )
			{
				std::vector<double> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					
					s.push_back( ParseDouble( payload[i] ) );
				}
				yaml[name] = s;
			}
			else if( CheckXmlType( payload, XmlRpc::XmlRpcValue::TypeString ) )
			{
				std::vector<std::string> s;
				for( int i = 0; i < payload.size(); i++ )
				{
					s.push_back( static_cast<std::string>( payload[i]) );
				}
				yaml[name] = s;
			}
			else
			{
				std::cerr << "Invalid array type." << std::endl;
			}
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeBoolean )
		{
			yaml[name] = static_cast<bool>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeInt )
		{
			yaml[name] = static_cast<int>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeDouble )
		{
			yaml[name] = static_cast<double>( payload );
		}
		else if( payload.getType() == XmlRpc::XmlRpcValue::TypeString )
		{
			yaml[name] = static_cast<std::string>( payload );
		}
		else
		{
			std::cerr << "Unsupported conversion type." << std::endl;
			continue;
		}
	}
	return yaml;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "uav_commander");
    ros::NodeHandle n;
    ros::NodeHandle n_priv("~");


    ////////////////////////////
    // Load object part types //
    ////////////////////////////



    std::vector<std::string> robot_ids;
    std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation;

    XmlRpc::XmlRpcValue swarm;
    n_priv.getParam("swarm", swarm);
    ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots.");
    std::map<std::string, XmlRpc::XmlRpcValue>::iterator i;
    for (i = swarm.begin(); i != swarm.end(); i++)
    {
        robot_ids.push_back(i->first);
        Eigen::Matrix<double,4,1, Eigen::DontAlign> pose;
        double x=i->second["pose"]["x"]["value"];
        double y=i->second["pose"]["y"]["value"];
        double z=i->second["pose"]["z"]["value"];
        double yaw=i->second["pose"]["yaw"]["value"];
        pose << x, y,z, yaw;
        desired_formation.push_back(pose);
    }

    SwarmFormationControl swarm_formation(n,robot_ids,desired_formation);

    ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10);
    geometry_msgs::PoseStamped goal_msg;
    goal_msg.pose.position.x=0.0;
    goal_msg.pose.position.y=0.0;
    goal_msg.pose.position.z=1.0;


    //    static tf::TransformBroadcaster brr;
    //    tf::Transform transform;
    //    int increment=1;
    //    tf::Quaternion q;
    //    q.setRPY(0, 0, 0);
    //    transform.setRotation(q);
    //    transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) );


    ros::AsyncSpinner spinner(4);
    spinner.start();


    ros::Rate loop_rate(20);
    while (ros::ok())
    {
        //        ++increment;

        //        transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) );
        //        transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) );

        //        q.setRPY(0, 0, 0+increment*0.001);
        //        transform.setRotation(q);
        //        brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal"));

        swarm_formation.updateCentroid();
        loop_rate.sleep();
        goal_msg.pose.position.x+=0.01;

        goal_msg.pose.position.z+=0.001;
        goal_pub.publish(goal_msg);

    }
    spinner.stop();

    return 0;
}

void WholeBodyController::loadParameterFiles()
{
    ros::NodeHandle n("~");
    std::string ns = ros::this_node::getName();
    XmlRpc::XmlRpcValue groups;
    typedef std::map<std::string, XmlRpc::XmlRpcValue>::iterator XmlRpcIterator;
    try
    {
        // ROBOT
        n.getParam("/whole_body_controller/collision_model", groups);
        for(XmlRpcIterator itrGroups = groups.begin(); itrGroups != groups.end(); ++itrGroups)
        {
            // COLLISION GROUP
            std::vector< RobotState::CollisionBody > robot_state_group;
            XmlRpc::XmlRpcValue group = itrGroups->second;
            for(XmlRpcIterator itrBodies = group.begin(); itrBodies != group.end(); ++itrBodies)
            {
                // COLLISION BODY
                XmlRpc::XmlRpcValue collisionBody = itrBodies->second;
                //cout << collisionBody["name"] << endl;

                RobotState::CollisionBody robotstate_collision_body;
                robotstate_collision_body.fromXmlRpc(collisionBody);

                // add the collision bodies to the group
                robot_state_group.push_back(robotstate_collision_body);
            }

            // add group of collision bodies to the robot
            robot_state_.robot_.groups.push_back(robot_state_group);
        }
        if (robot_state_.robot_.groups.size() == 0)
        {
            ROS_WARN("No collision model loaded");
        }

        XmlRpc::XmlRpcValue exclusion_groups;
        n.getParam("/whole_body_controller/exlusions_collision_calculation", exclusion_groups);
        for(XmlRpcIterator itrExclGr = exclusion_groups.begin(); itrExclGr != exclusion_groups.end(); ++itrExclGr)
        {
            XmlRpc::XmlRpcValue ExclGroup = itrExclGr->second;
            for(XmlRpcIterator itrExcl = ExclGroup.begin(); itrExcl != ExclGroup.end(); ++itrExcl)
            {
                XmlRpc::XmlRpcValue Excl = itrExcl->second;
                RobotState::Exclusion exclusion;
                exclusion.fromXmlRpc(Excl);

                robot_state_.exclusion_checks.checks.push_back(exclusion);
            }
        }

        if (robot_state_.exclusion_checks.checks.size() == 0)
        {
            ROS_WARN("No exclusions from self-collision avoindance checks");
        }
        else if (robot_state_.exclusion_checks.checks.size() > 0)
        {
            ROS_DEBUG("Exclusions from self-collision checks are: ");
            for (std::vector<RobotState::Exclusion>::iterator it = robot_state_.exclusion_checks.checks.begin(); it != robot_state_.exclusion_checks.checks.end(); ++it)
            {
                RobotState::Exclusion excl = *it;
                ROS_DEBUG("Name body A = %s", excl.name_body_A.c_str());
                ROS_DEBUG("Name body B = %s", excl.name_body_B.c_str());
            }
        }

    } catch(XmlRpc::XmlRpcException& ex)
    {
        std::cout << ex.getMessage() << std::endl;
    }

}

MidemUserInteraction::MidemUserInteraction(QMainWindow *parent) :
  QMainWindow(parent),
  ui(new Ui::MidemUserInteraction),
  quit_thread_(false),
  nhp_("~"),
  working_frame_("/base_link"),
  gesture_detector_loader_("midem_user_interaction","hg_gesture_detector::GestureDetector")
{
  ui->setupUi(this);
  dynamic_reconfigure::Server<midem_user_interaction::MidemUserInteractionConfig>::CallbackType f;
  f = boost::bind(&MidemUserInteraction::callbackConfig, this, _1, _2);
  reconfigure_server_.setCallback(f);

  arms_sub_ = nh_.subscribe("arms_msg", 1, &MidemUserInteraction::callbackArms, this);
  arm_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("arm_gestures", 1);

  skeletons_sub_ = nh_.subscribe("skeletons_msg", 1, &MidemUserInteraction::callbackSkeletons, this);
  skeleton_gesture_pub_ = nhp_.advertise<interaction_msgs::Gestures>("skeleton_gestures", 1);

  arms_syn_sub_.subscribe(nh_, "arms_msg", 10);
  skeletons_syn_sub_.subscribe(nh_, "skeletons_msg", 10);
  arm_skeleton_sync_.reset(new message_filters::Synchronizer<ArmBodyAppoxSyncPolicy>(ArmBodyAppoxSyncPolicy(10), arms_syn_sub_, skeletons_syn_sub_));
  arm_skeleton_sync_->registerCallback(boost::bind(&MidemUserInteraction::callbackArmsSkelentons, this, _1, _2));

  arm_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("arm_gesture_markers", 1);
  skeleton_gesture_markers_pub_ = nhp_.advertise<visualization_msgs::MarkerArray>("skeleton_gesture_markers", 1);

  XmlRpc::XmlRpcValue gesture_detector;
  nhp_.getParam("gesture_detector", gesture_detector);
  if(gesture_detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
  {
    ROS_ERROR("invalid YAML structure");
    return;
  }

  ROS_INFO_STREAM("load " << gesture_detector.size() << " gesture detector(s)");
  for(XmlRpc::XmlRpcValue::iterator it = gesture_detector.begin(); it != gesture_detector.end(); it++)
  {
    ROS_INFO_STREAM("detector name: " << it->first);
    XmlRpc::XmlRpcValue detector;
    nhp_.getParam("gesture_detector/" + it->first, detector);
    if(detector.getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_ERROR("invalid YAML structure");
      return;
    }

    if(detector.begin()->first != "type")
    {
      ROS_ERROR("invalid YAML structure");
      return;
    }

    ROS_INFO_STREAM("         type: " << detector.begin()->second);
    try
    {
      gesture_detector_map_[it->first] = gesture_detector_loader_.createInstance(detector.begin()->second);
      gesture_detector_map_[it->first]->setName(it->first);
      if(!gesture_detector_map_[it->first]->initialize())
      {
        ROS_ERROR_STREAM("Cannot initialize detector " << it->first);
        gesture_detector_map_.erase(it->first);
      }
    }
    catch(pluginlib::PluginlibException& ex)
    {
      ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
    }

  }
}

// WriteCameraFeaturesFromRosparam()
// Read ROS parameters from this node's namespace, and see if each parameter has a similarly named & typed feature in the camera.  Then set the
// camera feature to that value.  For example, if the parameter camnode/Gain is set to 123.0, then we'll write 123.0 to the Gain feature
// in the camera.
//
// Note that the datatype of the parameter *must* match the datatype of the camera feature, and this can be determined by
// looking at the camera's XML file.  Camera enum's are string parameters, camera bools are false/true parameters (not 0/1),
// integers are integers, doubles are doubles, etc.
//
void WriteCameraFeaturesFromRosparam(void)
{
	XmlRpc::XmlRpcValue	 			 xmlrpcParams;
	XmlRpc::XmlRpcValue::iterator	 iter;
    ArvGcNode						*pGcNode;
	GError							*error=NULL;


	global.phNode->getParam (ros::this_node::getName(), xmlrpcParams);


	if (xmlrpcParams.getType() == XmlRpc::XmlRpcValue::TypeStruct)
	{
		for (iter=xmlrpcParams.begin(); iter!=xmlrpcParams.end(); iter++)
		{
			std::string		key = iter->first;

			pGcNode = arv_device_get_feature (global.pDevice, key.c_str());
			if (pGcNode && arv_gc_feature_node_is_implemented (ARV_GC_FEATURE_NODE (pGcNode), &error))
			{
//				unsigned long	typeValue = arv_gc_feature_node_get_value_type((ArvGcFeatureNode *)pGcNode);
//				ROS_INFO("%s cameratype=%lu, rosparamtype=%d", key.c_str(), typeValue, static_cast<int>(iter->second.getType()));

				// We'd like to check the value types too, but typeValue is often given as G_TYPE_INVALID, so ignore it.
				switch (iter->second.getType())
				{
					case XmlRpc::XmlRpcValue::TypeBoolean://if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeBoolean))// && (typeValue==G_TYPE_INT64))
					{
						int			value = (bool)iter->second;
						arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value);
						ROS_INFO("Read parameter (bool) %s: %d", key.c_str(), value);
					}
					break;

					case XmlRpc::XmlRpcValue::TypeInt: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeInt))// && (typeValue==G_TYPE_INT64))
					{
						int			value = (int)iter->second;
						arv_device_set_integer_feature_value(global.pDevice, key.c_str(), value);
						ROS_INFO("Read parameter (int) %s: %d", key.c_str(), value);
					}
					break;

					case XmlRpc::XmlRpcValue::TypeDouble: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeDouble))// && (typeValue==G_TYPE_DOUBLE))
					{
						double		value = (double)iter->second;
						arv_device_set_float_feature_value(global.pDevice, key.c_str(), value);
						ROS_INFO("Read parameter (float) %s: %f", key.c_str(), value);
					}
					break;

					case XmlRpc::XmlRpcValue::TypeString: //if ((iter->second.getType()==XmlRpc::XmlRpcValue::TypeString))// && (typeValue==G_TYPE_STRING))
					{
						std::string	value = (std::string)iter->second;
						arv_device_set_string_feature_value(global.pDevice, key.c_str(), value.c_str());
						ROS_INFO("Read parameter (string) %s: %s", key.c_str(), value.c_str());
					}
					break;

					case XmlRpc::XmlRpcValue::TypeInvalid:
					case XmlRpc::XmlRpcValue::TypeDateTime:
					case XmlRpc::XmlRpcValue::TypeBase64:
					case XmlRpc::XmlRpcValue::TypeArray:
					case XmlRpc::XmlRpcValue::TypeStruct:
					default:
						ROS_WARN("Unhandled rosparam type in WriteCameraFeaturesFromRosparam()");
				}
			}
		}
	}
} // WriteCameraFeaturesFromRosparam()