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

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

int PlatformCtrlNode::init()
{
	std::string kinType;
	n.param<bool>("sendTransform",sendTransform,false);
	if(sendTransform)
	{
		ROS_INFO("platform ctrl node: sending transformation");
	} else {

		ROS_INFO("platform ctrl node: sending no transformation");
	}
	n.getParam("kinematics", kinType);

	if (kinType == "DiffDrive4W")
	{

		double wheelDiameter;
		double axisLength;
		DiffDrive4WKinematics* diffKin = new DiffDrive4WKinematics;
		kin = diffKin;
		if(n.hasParam("wheelDiameter"))
		{
			n.getParam("wheelDiameter",wheelDiameter);
			diffKin->setWheelDiameter(wheelDiameter);
			ROS_INFO("got wheeldieameter from config file");
		}
		else diffKin->setWheelDiameter(0.3);
		if(n.hasParam("robotWidth"))
		{
			n.getParam("robotWidth",axisLength);
			diffKin->setAxisLength(axisLength);
			ROS_INFO("got axis from config file");
		}
		else diffKin->setAxisLength(1);
	}
	else
	{
		ROS_ERROR("neo_PlatformCtrl-Error: unknown kinematic model");

	}
	if(kin == NULL) return 1;
	p.xAbs = 0; p.yAbs = 0; p.phiAbs = 0;
	topicPub_Odometry = n.advertise<nav_msgs::Odometry>("/odom",1);	
	topicSub_DriveState = n.subscribe("/joint_states",1,&PlatformCtrlNode::receiveOdo, this);
	topicPub_DriveCommands = n.advertise<trajectory_msgs::JointTrajectory>("/cmd_joint_traj",1);	
	topicSub_ComVel = n.subscribe("/cmd_vel",1,&PlatformCtrlNode::receiveCmd, this);
	return 0;
}

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

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

int NodeClass::init() 
{
	if (n.hasParam("ComPort"))
	{
		n.getParam("ComPort", sComPort);
		ROS_INFO("Loaded ComPort parameter from parameter server: %s",sComPort.c_str());
	}
	else
	{
		sComPort ="/dev/ttyUSB0";
		ROS_WARN("ComPort Parameter not available, using default Port: %s",sComPort.c_str());
	}

	n.param("message_timeout", relayboard_timeout_, 2.0);

	n.param("protocol_version", protocol_version_, 1);
    
	m_SerRelayBoard = new SerRelayBoard(sComPort, protocol_version_);
	ROS_INFO("Opened Relayboard at ComPort = %s", sComPort.c_str());

	m_SerRelayBoard->init();

	// Init member variable for EM State
	EM_stop_status_ = ST_EM_ACTIVE;
	duration_for_EM_free_ = ros::Duration(1);

	return 0;
}

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

bool setup(ros::NodeHandle nh){
    if (!nh.hasParam("/has_parameters")){
        return false;
    }
    nh.getParam("/rot_spread", rot_spread);
    nh.getParam("/vel_spread", vel_spread);
    nh.getParam("/updates_before_resample", updates_before_resample);
    nh.getParam("/times_to_resample", times_to_resample);
    nh.getParam("/prob_sigma", prob_sigma);
    nh.getParam("/maze_xmax", maze_xmax);
    nh.getParam("/maze_ymax", maze_ymax);

    std::vector<double> ir_positions_temp;

    for(int i=0;i<NUM_OBSERVATIONS;i++){
        std::string temp = "/ir_positions_";
        temp += boost::lexical_cast<std::string>(i+1);
        nh.getParam(temp, ir_positions_temp);
        sensor_positions[i][0] = ir_positions_temp[0];
        sensor_positions[i][1] = ir_positions_temp[1];
        sensor_positions[i][2] = ir_positions_temp[2];
    }
    bool known_pos;
    nh.getParam("/known_pos", known_pos);
    if(known_pos){
        nh.getParam("/start_x", position.x);
        nh.getParam("/start_y", position.y);
        nh.getParam("/start_theta", position.theta);
        init_known_pos(position.x, position.y, position.theta);
    }
    else{
        void init_un_known_pos(0.0,0.0,maze_xmax,maze_ymax);
    }
}

  void loadVehParam(const std::string& veh_type)
  {
    if (not nh_.hasParam(veh_type)){
      // Does not have such vehicle type defined
      return;
    }
    rvo_ros::AgentParam param;
    double neighborDist; nh_.param<double>(veh_type + "/neighborDist", neighborDist, 10);
    int maxNeighbors; nh_.param<int>(veh_type + "/maxNeighbors", maxNeighbors, 25);
    double timeHorizon; nh_.param<double>(veh_type + "/timeHorizon", timeHorizon, 1.5);
    double timeHorizonObst; nh_.param<double>(veh_type + "/timeHorizonObst", timeHorizonObst, 1.2);
    double radius; nh_.param<double>(veh_type + "/radius", radius, 0.4);
    double maxSpeed; nh_.param<double>(veh_type + "/maxSpeed", maxSpeed, 1.2);
    double maxAcc; nh_.param<double>(veh_type + "/maxAcc", maxAcc, 10.0);
    double lambdaValue; nh_.param<double>(veh_type + "/lambdaValue", lambdaValue, 0.6);
    
    param.agentType=rvo_ros::AgentParam::NORMAL;
    param.neighborDist = neighborDist;
    param.maxNeighbors = maxNeighbors;
    param.timeHorizon = timeHorizon;
    param.timeHorizonObst = timeHorizonObst;
    param.radius = radius;
    param.maxSpeed = maxSpeed;
    param.maxAcc = maxAcc;
    param.lambdaValue = lambdaValue;
    rvo_param_map_[veh_type] = param;

    // ROS_INFO_STREAM("[loadVehParam]:" << veh_type << ":" << param);
  }

  /****************************************************
   * @brief : Default constructor
   * @param : ros node handler
   ****************************************************/
  RobotObserver(ros::NodeHandle& node)
  {
    node_=node;
    
    // Getting robot's id from ros param
    if(node_.hasParam("my_robot_id"))
    {
      node_.getParam("my_robot_id",my_id_);
    } else {
      my_id_="PR2_ROBOT";
    }
    waiting_timer_ = node_.createTimer(ros::Duration(1.0), &RobotObserver::timerCallback, this, true);
    timer_on_=false;
    enable_event_=true;
    task_started_=false;
    // Advertise subscribers
    fact_list_sub_ = node_.subscribe("/agent_monitor/factList", 1, &RobotObserver::factListCallback, this);
    fact_area_list_sub_ = node_.subscribe("/area_manager/factList", 1, &RobotObserver::factListAreaCallback, this);
    human_list_sub_ = node_.subscribe("/pdg/humanList", 1, &RobotObserver::humanListCallback, this);
    object_list_sub_ = node_.subscribe("/pdg/objectList", 1, &RobotObserver::objectListCallback, this);
    current_action_list_sub_ = node_.subscribe("/human_monitor/current_humans_action", 1, &RobotObserver::CurrentActionListCallback, this);
    next_action_list_sub_ = node_.subscribe("/plan_executor/next_humans_actions", 1, &RobotObserver::NextActionListCallback, this);
    // Advertise publishers
    attention_vizu_pub_ = node_.advertise<geometry_msgs::PointStamped>("head_manager/attention_vizualisation", 5);

    init_action_client_ = new InitActionClient_t("pr2motion/Init", true);
    // Initialize action client for the action interface to the head controller
    head_action_client_ = new HeadActionClient_t("pr2motion/Head_Move_Target", true);
    // Connection to the pr2motion client
    connect_port_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/connect_port");
    head_stop_srv_ = node_.serviceClient<pr2motion::connect_port>("pr2motion/Head_Stop");
    ROS_INFO("[robot_observer] Waiting for pr2motion action server to start.");
    init_action_client_->waitForServer(); //will wait for infinite time
    head_action_client_->waitForServer(); //will wait for infinite time
    ROS_INFO("[robot_observer] pr2motion action server started.");

    pr2motion::InitGoal goal_init;
    init_action_client_->sendGoal(goal_init);

    pr2motion::connect_port srv;
    srv.request.local = "joint_state";
    srv.request.remote = "joint_states";
    if (!connect_port_srv_.call(srv)){
      ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
    }
    srv.request.local = "head_controller_state";
    srv.request.remote = "/head_traj_controller/state";
    if (!connect_port_srv_.call(srv)){
      ROS_ERROR("[robot_observer] Failed to call service pr2motion/connect_port");
    }
    pr2motion::Z_Head_SetMinDuration srv_MinDuration;
    srv_MinDuration.request.head_min_duration=0.6;
    if(!ros::service::call("/pr2motion/Z_Head_SetMinDuration",srv_MinDuration))
        ROS_ERROR("[robot_observer] Failed to call service /pr2motion/Z_Head_SetMinDuration");
    state_machine_ = new ObserverStateMachine(boost::cref(this));
    state_machine_->start();
    ROS_INFO("[robot_observer] Starting state machine, node ready !");
  }

void getParams(const ros::NodeHandle &nh)
{
    //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_BLOB_MAPPER_NODE_PUBLISH_FREQ);
        else
            ROS_WARN("No value set for %s/publish_freq. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ);
        PUBLISH_FREQ = DEFAULT_BLOB_MAPPER_NODE_PUBLISH_FREQ;
    }

    //number of blob sequences from blob_filter this node will buffer before it begins to drop them
    if (nh.getParam("sequence_poses_msg_buffer", SEQ_POSES_MSG_BUFFER))
    {
        ROS_INFO("Set %s/sequence_poses_msg_buffer to %d",nh.getNamespace().c_str(), SEQ_POSES_MSG_BUFFER);
    }
    else
    {
        if(nh.hasParam("sequences_msg_buffer"))
            ROS_WARN("%s/sequences_msg_buffer must be an integer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
        else
            ROS_WARN("No value set for %s/sequences_msg_buffer. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER);
        SEQ_POSES_MSG_BUFFER = DEFAULT_BLOB_MAPPER_NODE_SEQ_POSES_MSG_BUFFER;
    }

    //number of messages this node will queue for publishing before it drops 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_BLOB_MAPPER_NODE_MSG_QUEUE);
        else
            ROS_WARN("No value set for %s/msg_queue. Setting default value: %d",nh.getNamespace().c_str(), DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE);
        MSG_QUEUE = DEFAULT_BLOB_MAPPER_NODE_MSG_QUEUE;
    }
}

// Set Opencv Algorithm parameters from ROS parameter server
void setParameters( ros::NodeHandle& nodeHandle, cv::Ptr<cv::Algorithm>&& algorithm, const std::string& base_name )
{
  std::vector<cv::String> parameters;
  algorithm->getParams( parameters );

  for ( const auto& param : parameters )
  {
    if ( nodeHandle.hasParam(base_name + "/" + param) )
    {
      int param_type = algorithm->paramType( param );

      switch ( param_type )
      {
        case cv::Param::INT:
        {
          int val;
          nodeHandle.getParam(base_name + "/" + param, val);
          algorithm->set(param, val);
          std::cout << "  " << param << ": " << val << std::endl;
          break;
        }

        case cv::Param::BOOLEAN:
        {
          bool val;
          nodeHandle.getParam(base_name + "/" + param, val);
          algorithm->set(param, val);
          std::cout << "  " << param << ": " << val << std::endl;
          break;
        }

        case cv::Param::REAL:
        {
          double val;
          nodeHandle.getParam(base_name + "/" + param, val);
          algorithm->set(param, val);
          std::cout << "  " << param << ": " << val << std::endl;
          break;
        }

        case cv::Param::STRING:
        {
          std::string val;
          nodeHandle.getParam(base_name + "/" + param, val);
          algorithm->set(param, val);
          std::cout << "  " << param << ": " << val << std::endl;
          break;
        }

        default:
          ROS_ERROR_STREAM("unknown/unsupported parameter type for ");
          break;
      }
    }
  }
}

RosTopicParser::RosTopicParser(ros::NodeHandle nh, std::string topic_name){
  _topic_name = ""; _queue_size = 1;
  if (nh.hasParam(topic_name)){
    nh.getParam("/" + topic_name + "/" + NAME_KEY, _topic_name);
    nh.getParam("/" + topic_name + "/" + QUEUE_SIZE_KEY, _queue_size);
    ROS_INFO("Topic %s, queue size %i", _topic_name.c_str(), _queue_size);
  }else{
    ROS_WARN("Topic not found.");
  }
}

		T loadParam( std::string name, ros::NodeHandle &nh){
			T param;
			if (nh.hasParam( name )){
			    nh.getParam( name, param);
			    return param;
			} else {
			    std::cout << "Param " << name << " does not exist." << std::endl;
			    exit(0);
			}
	    }

static std::string get_cubin_path(const ros::NodeHandle& n, const char *default_path)
{
	std::string path;
	if (n.hasParam("/car_detector/cubin")) {
		n.getParam("/car_detector/cubin", path);
	} else {
		path = std::string(default_path);
	}

	return path;
}

//! Initialize plugin - called in server constructor
void srs_env_model::CPointCloudPlugin::init(ros::NodeHandle & node_handle)
{
//	PERROR( "Initializing PointCloudPlugin" );


	// Read parameters

	// Point cloud publishing topic name
	node_handle.param("pointcloud_centers_publisher", m_pcPublisherName, POINTCLOUD_CENTERS_PUBLISHER_NAME );

	// Point cloud subscribing topic name
	node_handle.param("pointcloud_subscriber", m_pcSubscriberName, SUBSCRIBER_POINT_CLOUD_NAME);

	// Point cloud limits
	node_handle.param("pointcloud_min_z", m_pointcloudMinZ, m_pointcloudMinZ);
	node_handle.param("pointcloud_max_z", m_pointcloudMaxZ, m_pointcloudMaxZ);

	// Contains input pointcloud RGB?
	if( node_handle.hasParam("input_has_rgb") )
	{
		node_handle.param("input_has_rgb", m_bUseRGB, m_bUseRGB );
		m_bRGB_byParameter = true;
	}

	// Create publisher
	m_pcPublisher = node_handle.advertise<sensor_msgs::PointCloud2> (m_pcPublisherName, 100, m_latchedTopics);


	// If should subscribe, create message filter and connect to the topic
	if( m_bSubscribe )
	{
		//PERROR("Subscribing to: " << m_pcSubscriberName );
		// Create subscriber
		m_pcSubscriber  = new message_filters::Subscriber<tIncommingPointCloud>(node_handle, m_pcSubscriberName, 1);

		if (!m_pcSubscriber)
		{
			ROS_ERROR("Not subscribed...");
			PERROR( "Not subscirbed to point clouds subscriber...");
		}

		// Create message filter
		m_tfPointCloudSub = new tf::MessageFilter<tIncommingPointCloud>( *m_pcSubscriber, m_tfListener, m_pcFrameId, 1);
		m_tfPointCloudSub->registerCallback(boost::bind( &CPointCloudPlugin::insertCloudCallback, this, _1));

		//std::cerr << "SUBSCRIBER NAME: " << m_pcSubscriberName << ", FRAMEID: " << m_pcFrameId << std::endl;
	}

	// Clear old pointcloud data
	m_data->clear();

//	PERROR( "PointCloudPlugin initialized..." );
}