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

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 GainClient::readGains(XmlRpc::XmlRpcValue& config,
                           sl_controller_msgs::CartesianGains& position_gains,
                           sl_controller_msgs::CartesianGains& force_gains)
{
  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Gains must be specified as an array");
    return false;
  }

  if (config.size() != 6)
  {
    ROS_ERROR("Gains must be be an array of six characters");
    return false;
  }

  char v[6];
  for (int i=0; i<6; ++i)
  {
    if (config[i].getType() != XmlRpc::XmlRpcValue::TypeString)
    {
      ROS_ERROR("Gains must be be an array of six characters");
      return false;
    }
    std::string s = config[i];
    v[i] = s.at(0);
    if (v[i] != 'f' && v[i] !='p' && v[i]!='0')
    {
      ROS_ERROR("Gains must be either 'p', 'f', or '0'");
      return false;
    }
  }

  setGainElement(v[0], position_gains.pos_x.p_gain, force_gains.pos_x.p_gain);
  setGainElement(v[1], position_gains.pos_y.p_gain, force_gains.pos_y.p_gain);
  setGainElement(v[2], position_gains.pos_z.p_gain, force_gains.pos_z.p_gain);
  setGainElement(v[3], position_gains.rot_x.p_gain, force_gains.rot_x.p_gain);
  setGainElement(v[4], position_gains.rot_y.p_gain, force_gains.rot_y.p_gain);
  setGainElement(v[5], position_gains.rot_z.p_gain, force_gains.rot_z.p_gain);

  setIGainsFromPGains(position_gains);
  setDGainsFromPGains(position_gains);
  setIGainsFromPGains(force_gains);
  setDGainsFromPGains(force_gains);
  return true;
}

bool NC2010TransformationSystem::initOneDimensionalTransformationSystemHelper(std::vector<dmp_lib::NC2010TSPtr>& transformation_systems,
                                                                                XmlRpc::XmlRpcValue transformation_systems_parameters_xml,
                                                                                ros::NodeHandle& node_handle,
                                                                                dmp_lib::TransformationSystem::IntegrationMethod integration_method)
{
  ROS_ASSERT(transformation_systems_parameters_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);
  for (int j = 0; j < transformation_systems_parameters_xml.size(); ++j)
  {
    ROS_DEBUG("Initializing NC2010 transformation system number >%i< from node handle.",j);

    // create transformation system
    dmp_lib::NC2010TSPtr transformation_system(new dmp_lib::NC2010TransformationSystem());

    XmlRpc::XmlRpcValue ts_xml = transformation_systems_parameters_xml[j];
    ROS_ASSERT(ts_xml.hasMember(transformation_system->getVersionString()));
    XmlRpc::XmlRpcValue nc2010_ts_xml = ts_xml[transformation_system->getVersionString()];

    double k_gain = 0;
    if (!usc_utilities::getParam(nc2010_ts_xml, "k_gain", k_gain))
    {
      return false;
    }
    double d_gain = dmp_lib::NC2010TransformationSystem::getDGain(k_gain);

    // create parameters
    dmp_lib::NC2010TSParamPtr parameters(new dmp_lib::NC2010TransformationSystemParameters());

    // initialize parameters
    ROS_VERIFY(parameters->initialize(k_gain, d_gain));

    // initialize base class
    ROS_VERIFY(TransformationSystem::initFromNodeHandle(parameters, ts_xml, node_handle));

    // create state
    dmp_lib::NC2010TSStatePtr state(new dmp_lib::NC2010TransformationSystemState());

    // initialize transformation system with parameters and state
    ROS_VERIFY(transformation_system->initialize(parameters, state, integration_method));
    ROS_VERIFY(transformation_system->setIntegrationMethod(integration_method));

    transformation_systems.push_back(transformation_system);
  }
  return true;
}

void StompOptimizationTask::setFeaturesFromXml(const XmlRpc::XmlRpcValue& features_xml)
{
  ROS_ASSERT (features_xml.getType() == XmlRpc::XmlRpcValue::TypeArray);

  std::vector<boost::shared_ptr<StompCostFeature> > features;

  for (int i=0; i<features_xml.size(); ++i)
  {
    XmlRpc::XmlRpcValue feature_xml = features_xml[i];

    ROS_ASSERT(feature_xml.hasMember("class") &&
               feature_xml["class"].getType() == XmlRpc::XmlRpcValue::TypeString);

    std::string class_name = feature_xml["class"];

    boost::shared_ptr<StompCostFeature> feature;
    try
    {
      feature = feature_loader_.createInstance(class_name);
    }
    catch (pluginlib::PluginlibException& ex)
    {
      ROS_ERROR("Couldn't load feature named %s", class_name.c_str());
      ROS_ERROR("Error: %s", ex.what());
      ROS_BREAK();
    }

    STOMP_VERIFY(feature->initialize(feature_xml, num_rollouts_+1,
                                     planning_group_name_,
                                     kinematic_model_,
                                     collision_robot_, collision_world_,
                                     collision_robot_df_, collision_world_df_));
    features.push_back(feature);
  }

  setFeatures(features);

}

inline bool parameterToColor(const ros::NodeHandle& node, const
    std::string& key, float* color) {
  XmlRpc::XmlRpcValue value;
  node.getParam(key, value);
  if (value.getType() == XmlRpc::XmlRpcValue::TypeArray) {
    if (value.size() == 3) {
      color[0] = static_cast<double>(value[0]);
      color[1] = static_cast<double>(value[1]);
      color[2] = static_cast<double>(value[2]);
   }
    else {
      ROS_WARN("Invalid array size for color parameter %s: %d", key.c_str(),
        (unsigned int)value.size());
      return false;
    }
  }
  else {
    ROS_WARN("Invalid type for color parameter %s: expecting array",
      key.c_str(), (unsigned int)value.size());
    return false;
  }
  
  return true;
}

  inline std::vector<std::string> getVectorParam(std::string name)
  {
    std::vector<std::string> values;
    XmlRpc::XmlRpcValue list;
    if (!root_nh_.getParamCached(name, list)) 
    {
      ROS_ERROR("Hand description: could not find parameter %s", name.c_str());
    }
    if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
      ROS_ERROR("Hand description: bad parameter %s", name.c_str());
    }
    //ROS_INFO_STREAM("Hand description vector param " << name << " resolved to:");
    for (int32_t i=0; i<list.size(); i++)
    {
      if (list[i].getType() != XmlRpc::XmlRpcValue::TypeString)
      {
	ROS_ERROR("Hand description: bad parameter %s", name.c_str());
      }
      values.push_back( static_cast<std::string>(list[i]) );
      //ROS_INFO_STREAM("  " << values.back());
    }
    return values;	
  }

TimeWarpNode::TimeWarpNode()
 : m_nh("~")
 , m_live(true)
 , m_tfHandler(this)
{
	m_srv = m_nh.advertiseService("control", &TimeWarpNode::handleTimeWarpControl, this);

	m_pub_clock = m_nh.advertise<rosgraph_msgs::Clock>("/clock", 1);

	m_timer = m_nh.createTimer(ros::Duration(0.1), boost::bind(&TimeWarpNode::update, this));
	m_timer.start();

	XmlRpc::XmlRpcValue list;
	if(m_nh.getParam("extra_topics", list))
	{
		ROS_INFO("Using extra_topics parameter");
		ROS_ASSERT(list.getType() == XmlRpc::XmlRpcValue::TypeArray);

		for(int i = 0; i < list.size(); ++i)
		{
			std::string name = static_cast<std::string>(list[i]);

			ROS_INFO("Warping extra topic '%s'", name.c_str());

			m_handlers.push_back(
				new TopicHandler(&m_nh, name)
			);
		}
	}

	m_handlers.push_back(
		new SmartTopicHandler<sensor_msgs::JointState>(&m_nh, "/joint_states")
	);

	m_topicThread = TopicThread::launch(m_nh, boost::bind(&TimeWarpNode::updateTopics, this, _1));
}

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

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

  }
}

void JointStateTorqueSensorController::addExtraJoints(const ros::NodeHandle& nh, sensor_msgs::JointState& msg)
{

  // Preconditions
  XmlRpc::XmlRpcValue list;
  if (!nh.getParam("extra_joints", list))
  {
    ROS_DEBUG("No extra joints specification found.");
    return;
  }

  if (list.getType() != XmlRpc::XmlRpcValue::TypeArray)
  {
    ROS_ERROR("Extra joints specification is not an array. Ignoring.");
    return;
  }

  for(std::size_t i = 0; i < list.size(); ++i)
  {
    if (list[i].getType() != XmlRpc::XmlRpcValue::TypeStruct)
    {
      ROS_ERROR_STREAM("Extra joint specification is not a struct, but rather '" << list[i].getType() <<
                       "'. Ignoring.");
      continue;
    }

    if (!list[i].hasMember("name"))
    {
      ROS_ERROR_STREAM("Extra joint does not specify name. Ignoring.");
      continue;
    }

    const std::string name = list[i]["name"];
    if (std::find(msg.name.begin(), msg.name.end(), name) != msg.name.end())
    {
      ROS_WARN_STREAM("Joint state interface already contains specified extra joint '" << name << "'.");
      continue;
    }

    const bool has_pos = list[i].hasMember("position");
    const bool has_vel = list[i].hasMember("velocity");
    const bool has_eff = list[i].hasMember("effort");

    const XmlRpc::XmlRpcValue::Type typeDouble = XmlRpc::XmlRpcValue::TypeDouble;
    if (has_pos && list[i]["position"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default position. Ignoring.");
      continue;
    }
    if (has_vel && list[i]["velocity"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default velocity. Ignoring.");
      continue;
    }
    if (has_eff && list[i]["effort"].getType() != typeDouble)
    {
      ROS_ERROR_STREAM("Extra joint '" << name << "' does not specify a valid default effort. Ignoring.");
      continue;
    }

    // State of extra joint
    const double pos = has_pos ? static_cast<double>(list[i]["position"]) : 0.0;
    const double vel = has_vel ? static_cast<double>(list[i]["velocity"]) : 0.0;
    const double eff = has_eff ? static_cast<double>(list[i]["effort"])   : 0.0;

    // Add extra joints to message
    msg.name.push_back(name);
    msg.position.push_back(pos);
    msg.velocity.push_back(vel);
    msg.effort.push_back(eff);
  }
}

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 &&
//.........这里部分代码省略.........

bool CartesianComplianceController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle) {
	using namespace XmlRpc;
	this->nodeHandle = nodeHandle;
	this->robotPtr = robotPtr;

	ROS_INFO("Initializing interaction control for the youbot arm...\n");

	// Gets all of the joint pointers from the RobotState to a joints vector
	XmlRpc::XmlRpcValue jointNames;
	if (!nodeHandle.getParam("joints", jointNames)) {
		ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray) {
		ROS_ERROR("Malformed joint specification.  (namespace: %s)", nodeHandle.getNamespace().c_str());
		return false;
	}

	for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i) {
		XmlRpcValue &name = jointNames[i];
		if (name.getType() != XmlRpcValue::TypeString) {
			ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", nodeHandle.getNamespace().c_str());
			return false;
		}

		pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
		if (jointStatePtr == NULL) {
			ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}

		joints.push_back(jointStatePtr);
	}

	// Ensures that all the joints are calibrated.
	for (unsigned int i = 0; i < joints.size(); ++i) {
		if (!joints[i]->calibrated_) {
			ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
			return false;
		}
	}

	// Initializing target efforts vector
	targetEfforts.resize(joints.size());

	// Subscribing for an input pose command
	subscriber = nodeHandle.subscribe("command", 1, &CartesianComplianceController::positionCommand, this);

	// Initializing 20Sim controller
	init20SimController();

	// Initializing twist publisher for the base
	ROS_INFO("base_confghfhgtroller/command\n");
	baseTwist.reset(new realtime_tools::RealtimePublisher<geometry_msgs::Twist > (nodeHandle, "base_controller/command", 1));
	baseTwist->lock();

	subscriberOdometry = nodeHandle.subscribe("base_odometry/odometry", 1, &CartesianComplianceController::odometryCommand, this);


	// Initializing debug output
	debugInfo->init();

	return true;
}

bool JointPositionController::init(pr2_mechanism_model::RobotState *robotPtr, ros::NodeHandle &nodeHandle)
{
    using namespace XmlRpc;
    this->nodeHandle = nodeHandle;
    this->robotPtr = robotPtr;



    ROS_DEBUG("Initializing joint position control...\n");

    // Gets all of the joint pointers from the RobotState to a joints vector
    XmlRpc::XmlRpcValue jointNames;
    if (!nodeHandle.getParam("joints", jointNames))
    {
        ROS_ERROR("No joints given. (namespace: %s)", nodeHandle.getNamespace().c_str());
        return false;
    }

    if (jointNames.getType() != XmlRpc::XmlRpcValue::TypeArray)
    {
        ROS_ERROR("Malformed joint specification.  (namespace: %s)", nodeHandle.getNamespace().c_str());
        return false;
    }

    for (unsigned int i = 0; i < static_cast<unsigned int> (jointNames.size()); ++i)
    {
        XmlRpcValue &name = jointNames[i];
        if (name.getType() != XmlRpcValue::TypeString)
        {
            ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", nodeHandle.getNamespace().c_str());
            return false;
        }

        pr2_mechanism_model::JointState *jointStatePtr = robotPtr->getJointState((std::string)name);
        if (jointStatePtr == NULL)
        {
            ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name).c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }

        joints.push_back(jointStatePtr);
    }

    // Ensures that all the joints are calibrated.
    for (unsigned int i = 0; i < joints.size(); ++i)
    {
        if (!joints[i]->calibrated_)
        {
            ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }
    }

    // Initializing targetVelocities vector
    targetPositions.resize(joints.size());

    // Sets up pid controllers for all of the joints from yaml file
    std::string gainsNS;

    if (!nodeHandle.getParam("gains", gainsNS))
        gainsNS = nodeHandle.getNamespace() + "/gains";

    ROS_DEBUG("gains: %s\n", gainsNS.c_str());

    pids.resize(joints.size());

    ROS_DEBUG("joints.size() = %d \n", joints.size());

    for (unsigned int i = 0; i < joints.size(); ++i)
    {
	ROS_DEBUG("processing  %s/%s \n", gainsNS.c_str(), joints[i]->joint_->name.c_str());

        if (!pids[i].init(ros::NodeHandle(gainsNS + "/" + joints[i]->joint_->name)))
        {
            ROS_ERROR("Can't setup PID for the joint %s. (namespace: %s)", joints[i]->joint_->name.c_str(), nodeHandle.getNamespace().c_str());
            return false;
        }

        double p, i, d, i_max, i_min;
        pids[i].getGains(p, i, d, i_max, i_min);
        ROS_DEBUG("PID for joint %s: p=%f, i=%f, d=%f, i_max=%f, i_min=%f\n", joints[i]->joint_->name.c_str(), p, i, d, i_max, i_min);
    }

    subscriber = nodeHandle.subscribe("position_command", 1, &JointPositionController::positionCommand, this);

    return true;
}

    bool initialize()
    {
        XmlRpc::XmlRpcValue val;
        std::vector<std::string> static_joints;
        
        if (private_nh_.getParam("static_joints", val))
        {
            if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
            {
                ROS_ERROR("static_joints parameter is not a list");
                return false;
            }
            
            for (int i = 0; i < val.size(); ++i)
            {
                static_joints.push_back(static_cast<std::string>(val[i]));
            }
        }
        
        std::vector<std::string> controller_names;
        
        if (private_nh_.getParam("controllers", val))
        {
            if (val.getType() != XmlRpc::XmlRpcValue::TypeArray)
            {
                ROS_ERROR("controllers parameter is not a list");
                return false;
            }
            
            for (int i = 0; i < val.size(); ++i)
            {
                controller_names.push_back(static_cast<std::string>(val[i]));
            }
        }
        
        int num_static = static_joints.size();
        int num_controllers = controller_names.size();
        int num_total = num_static + num_controllers;
        
        msg_.name.resize(num_total);
        msg_.position.resize(num_total);
        msg_.velocity.resize(num_total);
        msg_.effort.resize(num_total);
        
        for (int i = 0; i < num_static; ++i)
        {
            msg_.name[i] = static_joints[i];
            msg_.position[i] = 0.0;
            msg_.velocity[i] = 0.0;
            msg_.effort[i] = 0.0;
        }
        
        controller_state_subs_.resize(num_controllers);
        
        for (int i = 0; i < num_controllers; ++i)
        {
            controller_state_subs_[i] =
				nh_.subscribe<dynamixel_hardware_interface::JointState>(controller_names[i] +  "/state", 100,
                boost::bind(&JointStateAggregator::processControllerState, this, _1, i+num_static));
        }
        
        for (int i = 0; i < num_controllers; ++i)
        {
            ros::topic::waitForMessage<dynamixel_hardware_interface::JointState>(controller_names[i] +  "/state");
        }
        
        joint_states_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 100);
        
        return true;
    }

bool GridMapVisualization::readParameters()
{
  nodeHandle_.param("grid_map_topic", mapTopic_, string("/grid_map"));

  double activityCheckRate;
  nodeHandle_.param("activity_check_rate", activityCheckRate, 2.0);
  activityCheckDuration_.fromSec(1.0 / activityCheckRate);
  ROS_ASSERT(!activityCheckDuration_.isZero());

  // Configure the visualizations from a configuration stored on the parameter server.
  XmlRpc::XmlRpcValue config;
  if (!nodeHandle_.getParam(visualizationsParameter_, config)) {
    ROS_WARN(
        "Could not load the visualizations configuration from parameter %s,are you sure it"
        "was pushed to the parameter server? Assuming that you meant to leave it empty.",
        visualizationsParameter_.c_str());
    return false;
  }

  // Verify proper naming and structure,
  if (config.getType() != XmlRpc::XmlRpcValue::TypeArray) {
    ROS_ERROR("%s: The visualization specification must be a list, but it is of XmlRpcType %d",
              visualizationsParameter_.c_str(), config.getType());
    ROS_ERROR("The XML passed in is formatted as follows:\n %s", config.toXml().c_str());
    return false;
  }

  // Iterate over all visualizations (may be just one),
  for (unsigned int i = 0; i < config.size(); ++i) {
    if (config[i].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
      ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
                visualizationsParameter_.c_str(), config[i].getType());
      return false;
    } else if (!config[i].hasMember("type")) {
      ROS_ERROR("%s: Could not add a visualization because no type was given",
                visualizationsParameter_.c_str());
      return false;
    } else if (!config[i].hasMember("name")) {
      ROS_ERROR("%s: Could not add a visualization because no name was given",
                visualizationsParameter_.c_str());
      return false;
    } else {
      //Check for name collisions within the list itself.
      for (int j = i + 1; j < config.size(); ++j) {
        if (config[j].getType() != XmlRpc::XmlRpcValue::TypeStruct) {
          ROS_ERROR("%s: Visualizations must be specified as maps, but they are XmlRpcType:%d",
                    visualizationsParameter_.c_str(), config[j].getType());
          return false;
        }

        if (!config[j].hasMember("name")
            || config[i]["name"].getType() != XmlRpc::XmlRpcValue::TypeString
            || config[j]["name"].getType() != XmlRpc::XmlRpcValue::TypeString) {
          ROS_ERROR("%s: Visualizations names must be strings, but they are XmlRpcTypes:%d and %d",
                    visualizationsParameter_.c_str(), config[i].getType(), config[j].getType());
          return false;
        }

        std::string namei = config[i]["name"];
        std::string namej = config[j]["name"];
        if (namei == namej) {
          ROS_ERROR("%s: A visualization with the name '%s' already exists.",
                    visualizationsParameter_.c_str(), namei.c_str());
          return false;
        }
      }
    }

    // Make sure the visualization has a valid type.
    if (!factory_.isValidType(config[i]["type"])) {
      ROS_ERROR("Could not find visualization of type '%s'.", std::string(config[i]["type"]).c_str());
      return false;
    }
  }

  for (int i = 0; i < config.size(); ++i) {
    std::string type = config[i]["type"];
    std::string name = config[i]["name"];
    auto visualization = factory_.getInstance(type, name);
    visualization->readParameters(config[i]);
    visualizations_.push_back(visualization);
    ROS_INFO("%s: Configured visualization of type '%s' with name '%s'.",
             visualizationsParameter_.c_str(), type.c_str(), name.c_str());
  }

  return true;
}