C++ NodeHandle::getParam方法代码示例

bool ROSnode::Prepare() {
  seq = 0;
  //Retrieve parameters
  if (Handle.getParam("device", dev)) {
    ROS_INFO("Node %s: retrieved parameter device.", ros::this_node::getName().c_str());
  }
  else {
    ROS_FATAL("Node %s: unable to retrieve parameter device.", ros::this_node::getName().c_str());
    return false;
  }
  
  
  if (Handle.getParam("rate", rate)) {
    ROS_INFO("Node %s: retrieved parameter rate.", ros::this_node::getName().c_str());
  }
  else {
    ROS_WARN("Node %s: unable to retrieve parameter rate. Setting default value to 20", ros::this_node::getName().c_str());
    rate = 20;
  }

  PublisherMag = Handle.advertise<sensor_msgs::MagneticField>("mag", 50);
  PublisherImu = Handle.advertise<sensor_msgs::Imu>("imu", 50);

  driver.setFrequency(rate);

  //Open and initialize the device
  if(!driver.open(dev) ||
     !driver.setReadingMode(xsens_imu::CAL_AND_ORI_DATA) ||
     !driver.setTimeout(100))
    return false;
  
  ROS_INFO("Node %s ready to run.", ros::this_node::getName().c_str());
  return true;
}

void GenericHWInterface::loadURDF(ros::NodeHandle &nh, std::string param_name)
{
  std::string urdf_string;
  urdf_model_ = new urdf::Model();

  // search and wait for robot_description on param server
  while (urdf_string.empty() && ros::ok())
  {
    std::string search_param_name;
    if (nh.searchParam(param_name, search_param_name))
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << search_param_name);
      nh.getParam(search_param_name, urdf_string);
    }
    else
    {
      ROS_INFO_STREAM_NAMED("generic_hw_interface", "Waiting for model URDF on the ROS param server at location: " <<
                            nh.getNamespace() << param_name);
      nh.getParam(param_name, urdf_string);
    }

    usleep(100000);
  }

  if (!urdf_model_->initString(urdf_string))
    ROS_ERROR_STREAM_NAMED("generic_hw_interface", "Unable to load URDF model");
  else
    ROS_DEBUG_STREAM_NAMED("generic_hw_interface", "Received URDF from param server");
}

bool RTKRobotArm::_initialize(const ros::NodeHandle& n) {
	bool ret=true;

	// path to RTK format data folder. Keep robots here!
	if(!n.getParam("data_path", data_path)) {
		ROS_ERROR("Must provide data_path!");
		ret = false;
	} else {
		ROS_INFO_STREAM("Data Folder: "<<data_path);
	}

	// RTK format robot name
	if(!n.getParam("robot_name", rob_name)) {
		ROS_ERROR("Must provide robot_name!");
		ret = false;
	} else {
		ROS_INFO_STREAM("Robot: "<<rob_name);
	}

	// EE link name in the structure.xml
	if(!n.getParam("ee_link", ee_link)) {
		ROS_ERROR("Must provide ee_link!");
		ret = false;
	} else {
		ROS_INFO_STREAM("EELink: "<<ee_link);
	}

	if(!n.getParam("max_jvel_deg", max_jvel)) {
		max_jvel = DEFAULT_MAX_JVEL_DEG;
	}
	ROS_INFO_STREAM("Max. Joint Vel. (deg): "<<max_jvel);

	return ret;
}

bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
{
    std::string urdf_xml,full_urdf_xml;
    node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
    node_handle.searchParam(urdf_xml,full_urdf_xml);
    TiXmlDocument xml;
    ROS_DEBUG("Reading xml file from parameter server\n");
    std::string result;
    if (node_handle.getParam(full_urdf_xml, result))
        xml.Parse(result.c_str());
    else
    {
        ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
        return false;
    }
    xml_string = result;
    TiXmlElement *root_element = xml.RootElement();
    TiXmlElement *root = xml.FirstChildElement("robot");
    if (!root || !root_element)
    {
        ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
        exit(1);
    }
    robot_model.initXml(root);
    if (!node_handle.getParam("root_name", root_name)) {
        ROS_FATAL("No root name found on parameter server");
        return false;
    }
    if (!node_handle.getParam("tip_name", tip_name)) {
        ROS_FATAL("No tip name found on parameter server");
        return false;
    }
    return true;
}

 void GetTuningParameters()
 {
     n.getParam("KPR",KPR);
     n.getParam("KPL",KPL);
     n.getParam("KIL",KIL);
     n.getParam("KIR",KIR);
     n.getParam("KDR",KDR);
     n.getParam("KDL",KDL);
 }

void CalcMin::chatterCallback1(const fts_Omega160::fts msg)
{
int data1;
int data2;
int data3;
int data4;
int stop;

  n.setParam("/stop", 0);
  req.data = std::vector<int>(8);
  req.data[4] = msg.Fx;
  req.data[5] = msg.Fy;
  req.data[6] = msg.Fz;

	if (n.getParam("/data1", data1)){
		  req.data[0] = data1;
		}
	if (n.getParam("/data2", data2)){
		  req.data[1] = data2;
		}
	if (n.getParam("/data3", data3)){
		  req.data[2] = data3;
		}
	if (n.getParam("/data4", data4)){
		  req.data[3] = data4;
		}
	if (n.getParam("/stop", stop)){
		  req.data[7] = stop;
		}


  pub = n.advertise<std_msgs::Int32MultiArray>("/min_pub",1000); 
  pub.publish(req);
}

geometry_msgs::PoseStamped getCamPose(ros::NodeHandle n)
{
    geometry_msgs::PoseStamped cam_pose;

    //set orientation
    cam_pose.pose.orientation.w = 1;

    if (!n.getParam("/suturo_manipulation_tf_publisher/cam_frame", cam_pose.header.frame_id))
    {
        ROS_ERROR_STREAM("Failed to Frame for Cam.");
    }
    if (!n.getParam("/suturo_manipulation_tf_publisher/cam_x", cam_pose.pose.position.x))
    {
        ROS_ERROR_STREAM("Failed to get x coordinate for Cam.");
    }
    if (!n.getParam("/suturo_manipulation_tf_publisher/cam_y", cam_pose.pose.position.y))
    {
        ROS_ERROR_STREAM("Failed to get y coordinate for Cam.");
    }
    if (!n.getParam("/suturo_manipulation_tf_publisher/cam_z", cam_pose.pose.position.z))
    {
        ROS_ERROR_STREAM("Failed to get z coordinate for Cam.");
    }

    return cam_pose;
}

void global_path::getparameters(ros::NodeHandle n)
{
  if (n.getParam("dbname", s_dbname))
    ROS_INFO("Got param: %s", s_dbname.c_str());
  else
    ROS_ERROR("Failed to get param 'dbname'");

  if (n.getParam("user", s_user))
    ROS_INFO("Got param: %s", s_user.c_str());
  else
    ROS_ERROR("Failed to get param 'user'");

  if (n.getParam("password", s_password))
    ROS_INFO("Got param: %s", s_password.c_str());
  else
    ROS_ERROR("Failed to get param 'password'");

  if (n.getParam("hostaddr", s_hostaddr))
    ROS_INFO("Got param: %s", s_hostaddr.c_str());
  else
    ROS_ERROR("Failed to get param 'hostaddr'");

  if (n.getParam("port", s_port))
    ROS_INFO("Got param: %d", s_port);
  else
    ROS_ERROR("Failed to get param 'port'");
}

	void IkJointControllerClass::getParams(ros::NodeHandle& n) {
		ROS_INFO("getting gains params and join name params");

		XmlRpc::XmlRpcValue joint_names;  //array of 7 joint names

		if (!n.getParam("joints", joint_names)){
		    ROS_ERROR("No joints given. (namespace: %s)", n.getNamespace().c_str());
		}

		if ((joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) || (joint_names.size() != 7)){
		    ROS_ERROR("Malformed joint specification.  (namespace: %s)", n.getNamespace().c_str());
		}

		std::string joint_param_ns;
		for (int i = 0; i < 7; i++){
			XmlRpc::XmlRpcValue &name_value = joint_names[i];
		    if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString){
		    	ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)", n.getNamespace().c_str());
		    }
		    joint_names_[i] = ((std::string)name_value).c_str();

		    joint_param_ns = std::string("gains/") + joint_names_[i];
			n.getParam(joint_param_ns + "/p", p_[i]);
			n.getParam(joint_param_ns + "/d", d_[i]);

			ROS_INFO("NEW YAML got %d-th joint name %s with (p,d) gains parameters: (%f,%f)",
					i,
					joint_names_[i].c_str(),
					p_[i],
					d_[i]);
		}

    }

void CollisionPluginLoader::setupScene(
  ros::NodeHandle& nh,
  const planning_scene::PlanningScenePtr& scene)
{
  if (!scene)
    return;

  std::string param_name;
  std::string collision_detector_name;

  if (nh.searchParam("collision_detector", param_name))
  {
    nh.getParam(param_name, collision_detector_name);
  }
  else if (nh.hasParam("/move_group/collision_detector"))
  {
    // Check for existence in move_group namespace
    // mainly for rviz plugins to get same collision detector.
    nh.getParam("/move_group/collision_detector", collision_detector_name);
  }
  else
  {
    return;
  }

  if (collision_detector_name == "")
  {
    // This is not a valid name for a collision detector plugin
    return;
  }

  activate(collision_detector_name, scene, true);
  ROS_INFO_STREAM("Using collision detector:" << scene->getActiveCollisionDetectorName().c_str());
}

 bool spin()
 {
     ROS_INFO("camera node is running.");
     h264Server.start();
     while (node.ok())
     {
         // Process any pending service callbacks
         ros::spinOnce();
         std::string newResolution;
         node.getParam("resolution", newResolution);
         int newWidth = std::stoi(newResolution.substr(0, newResolution.find('x')));
         int newHeight = std::stoi(newResolution.substr(newResolution.find('x') + 1));
         std::string newVideoDevice;
         node.getParam("video_device", newVideoDevice);
         bool newDetectorEnabled;
         node.getParam("detector_enabled", newDetectorEnabled);
         if (newVideoDevice != videoDevice || 
                 newDetectorEnabled != detectorEnabled ||
                 newWidth != cameraWidth ||
                 newHeight != cameraHeight) {
             initCameraAndEncoders();
         }
         if(!sendPreview()) {
             // Sleep and hope the camera recovers
             usleep(1000*1000);
         }
         // Run at 1kHz
         usleep(1000);
     }
     h264Server.stop();
     return true;
 }

void Adventurer::init()
{
    // strings temporaires
    std::string sSubscribeName,
                sRightWheel,
                sLeftWheel;

    // essaye de récupérer le nom du topic sur lequel on recoie les ordres
    // si aucun parametre n'est pas trouvé, on utilise une valeur par defaut
    if (!m_NodeHandle.getParam("sub_name", sSubscribeName))
        sSubscribeName = "adventurer_order"; // valeur par defaut
    
    // création du subscriber pour recevoir les ordres
    // on passe en parametre la callback (order_cb) et l'objet qui l'appellera (this)
    m_OrderSubscriber = m_NodeHandle.subscribe(sSubscribeName, 1, &Adventurer::order_cb, this);
    // création du publisher pour envoyer des ordres au robot
    m_WheelsPublisher = m_NodeHandle.advertise<nxt_msgs::JointCommand>("joint_command", 1);

    // cf. ligne 42
    if (!m_NodeHandle.getParam("right_wheel", sRightWheel))
        sRightWheel = "motor_r";
    if (!m_NodeHandle.getParam("left_wheel", sLeftWheel))
        sLeftWheel  = "motor_l";

    m_sRightWheel = sRightWheel;
    m_sLeftWheel  = sLeftWheel;
}

void getParams(const ros::NodeHandle &nh)
{
    //avg accel update period (number of msgs until next localization udpate)
    if (nh.getParam("update_period", UPDATE_PERIOD))
    {
        ROS_INFO("Set %s/update_period to %d",nh.getNamespace().c_str(), UPDATE_PERIOD);
    }
    else
    {
        if(nh.hasParam("update_period"))
            ROS_WARN("%s/update_period must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
        else
            ROS_WARN("No value set for %s/update_period. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_UPDATE_PERIOD);
        UPDATE_PERIOD = DEFAULT_LOC_NODE_UPDATE_PERIOD;
    }

    //frequency this node publishes a new topic
    if (nh.getParam("publish_freq", PUBLISH_FREQ))
    {
        ROS_INFO("Set %s/publish_freq to %d",nh.getNamespace().c_str(), PUBLISH_FREQ);
    }
    else
    {
        if(nh.hasParam("publish_freq"))
            ROS_WARN("%s/publish_freq must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
        else
            ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_PUBLISH_FREQ);
        PUBLISH_FREQ = DEFAULT_LOC_NODE_PUBLISH_FREQ;
    }

    //number of states from coax_filter this node will buffer before it begins to drop them
    if (nh.getParam("fstate_msg_buffer", FSTATE_MSG_BUFFER))
    {
        ROS_INFO("Set %s/fstate_msg_buffer to %d",nh.getNamespace().c_str(), FSTATE_MSG_BUFFER);
    }
    else
    {
        if(nh.hasParam("fstate_msg_buffer"))
            ROS_WARN("%s/fstate_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
        else
            ROS_WARN("No value set for %s/fstate_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER);
        FSTATE_MSG_BUFFER = DEFAULT_LOC_NODE_FSTATE_MSG_BUFFER;
    }

    //number of messages this node will queue for publishing before droping old data
    if (nh.getParam("msg_queue", MSG_QUEUE))
    {
        ROS_INFO("Set %s/msg_queue to %d",nh.getNamespace().c_str(), MSG_QUEUE);
    }
    else
    {
        if(nh.hasParam("msg_queue"))
            ROS_WARN("%s/msg_queue must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
        else
            ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_LOC_NODE_MSG_QUEUE);
        MSG_QUEUE = DEFAULT_LOC_NODE_MSG_QUEUE;
    }
}

void
fillCameraInfoMessage (ros::NodeHandle nh, std::string base_name, sensor_msgs::CameraInfo& camera_info_msg )
{
  // Read calibration data and fill camera_info message:
  int width, height;
  nh.param(base_name + "/image_width", width, 640);
  nh.param(base_name + "/image_height", height, 480);
  camera_info_msg.width = width;
  camera_info_msg.height = height;

  std::string distortion_model;
  if (not nh.getParam (base_name + "/distortion_model", distortion_model))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  camera_info_msg.distortion_model = distortion_model;

  std::vector<float> camera_matrix;
  if (not nh.getParam (base_name + "/camera_matrix/data", camera_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < camera_matrix.size(); i++)
  {
    camera_info_msg.K[i] = camera_matrix[i];
  }

  std::vector<float> rectification_matrix;
  if (not nh.getParam (base_name + "/rectification_matrix/data", rectification_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < rectification_matrix.size(); i++)
  {
    camera_info_msg.R[i] = rectification_matrix[i];
  }

  std::vector<float> projection_matrix;
  if (not nh.getParam (base_name + "/projection_matrix/data", projection_matrix))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < projection_matrix.size(); i++)
  {
    camera_info_msg.P[i] = projection_matrix[i];
  }

  std::vector<float> distortion_coefficients;
  if (not nh.getParam (base_name + "/distortion_coefficients/data", distortion_coefficients))
  {
    ROS_ERROR("Stereo calibration parameters not found!");
  }
  for (unsigned int i = 0; i < distortion_coefficients.size(); i++)
  {
    camera_info_msg.D.push_back(distortion_coefficients[i]);
  }
}

bool Kinematics::init() {
    // Get URDF XML
    std::string urdf_xml, full_urdf_xml;
    nh.param("urdf_xml",urdf_xml,std::string("robot_description"));
    nh.searchParam(urdf_xml,full_urdf_xml);
    ROS_DEBUG("Reading xml file from parameter server");
    std::string result;
    if (!nh.getParam(full_urdf_xml, result)) {
        ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
        return false;
    }
 
    std::string kinematics_service_name;
    if (!nh_private.getParam("kinematics_service_name", kinematics_service_name)) {
        ROS_FATAL("GenericIK: kinematics service name not found on parameter server");
        return false;
    }
    // Get Root and Tip From Parameter Service
    if (!nh.getParam(kinematics_service_name+"/manipulator/root_name", root_name)) {
        ROS_FATAL("GenericIK: No root name found on parameter server");
        return false;
    }
    if (!nh.getParam(kinematics_service_name+"/manipulator/tip_name", tip_name)) {
        ROS_FATAL("GenericIK: No tip name found on parameter server");
        return false;
    }

    // Load and Read Models
    if (!loadModel(result)) {
        ROS_FATAL("Could not load models!");
        return false;
    }

    // Get Solver Parameters
    int maxIterations;
    double epsilon;

    nh_private.param("maxIterations", maxIterations, 1000);
    nh_private.param("epsilon", epsilon, 1e-2); // .01

    // Build Solvers
    fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
    ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);

    ik_solver_pos = new KDL::ChainIkSolverPos_NR_JL(chain, joint_min, joint_max, *fk_solver, *ik_solver_vel, maxIterations, epsilon);

    ROS_INFO("Advertising services");
    fk_service = nh_private.advertiseService(FK_SERVICE,&Kinematics::getPositionFK,this);

    // ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::getPositionIK,this);
    ik_service = nh_private.advertiseService(IK_SERVICE,&Kinematics::searchPositionIK,this);

    ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE,&Kinematics::getIKSolverInfo,this);
    fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE,&Kinematics::getFKSolverInfo,this);

    return true;
}