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

void ApproximateValueManager::Initialize( BroadcastMultiReceiver::Ptr inputs,
                                          ros::NodeHandle& ph )
{
	WriteLock lock( _mutex );

	_receiver = inputs;
	unsigned int inputDim = _receiver->GetDim();
	ros::NodeHandle vh( ph.resolveName( "approximator" ) );
	_value.Initialize( inputDim, vh );

	_getParamServer = ph.advertiseService( "get_params", &ApproximateValueManager::GetParamsCallback, this );
	_setParamServer = ph.advertiseService( "set_params", &ApproximateValueManager::SetParamsCallback, this );

	std::string valueName;
	GetParamRequired( ph, "value_name", valueName );
	register_lookup_target( ph, valueName );

	ValueInfo info;
	info.inputDim = _receiver->GetDim();
	info.paramQueryService = ph.resolveName( "get_params" );
	info.paramSetService = ph.resolveName( "set_params" );
	GetParamRequired( vh, "", info.approximatorInfo );
	if( !_infoManager.WriteMemberInfo( valueName, info, true, ros::Duration( 10.0 ) ) )
	{
		throw std::runtime_error( "Could not write value info!" );
	}
}

  TransformPointcloudNode  (ros::NodeHandle &n) : nh_(n)
  {
    // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
    nh_.param("input_cloud_topic", input_cloud_topic_, std::string("cloud_pcd"));
    output_cloud_topic_ = input_cloud_topic_ + "_transformed";
    nh_.param("to_frame", to_frame_, std::string("base_link"));
    nh_.param("point_cloud_name", point_cloud_name_, std::string("pc"));
    nh_.param("save_point_cloud", save_point_cloud_, false);

    sub_ = nh_.subscribe (input_cloud_topic_, 1,  &TransformPointcloudNode::cloud_cb, this);
    ROS_INFO ("[TransformPointcloudNode:] Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
    pub_ = nh_.advertise<sensor_msgs::PointCloud2>(output_cloud_topic_, 1);
    ROS_INFO ("[TransformPointcloudNode:] Will be publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
  }

 // constructor
 online_IPM(ros::NodeHandle nh, int ow, int oh, double f_u, double f_v, double c_u, double c_v, double deg, double cam_h, std::string camera_name)
     : nh_(nh), priv_nh_("~"),ipMapper(ow,oh,f_u,f_v,c_u,c_v,deg,cam_h),it(nh_)
 {
     read_images_= nh_.subscribe(nh_.resolveName(camera_name), 1,&online_IPM::publish_remapper,this);
     pub_mapped_images_= it.advertise("/camera/ground_image_ipmapped", 1);
     //ipMapper.initialize(200,PROJECTED_IMAGE_HEIGTH, fu, fv, cx, cy, pitch);
 }

 void
 setupPubs()
 {
   //look up remapping
   std::string topic = nh_.resolveName(topic_, true);
   pub_ = nh_.advertise<MessageT>(topic, queue_size_, latched_);
   ROS_INFO_STREAM("publishing to topic:" << topic);
 }

    // constructor
    PoseTracker(ros::NodeHandle nh, int argc,char** argv) : nh_(nh), priv_nh_("~"), filter_()
    {
        priv_nh_.param<std::string>("reference_frame", reference_frame_, "");
        priv_nh_.param<std::string>("object_frame", object_frame_, "");

        // get the led location in local frame from ros param server
        nh_.getParam(object_frame_ + std::string("_leds"), leds_);
		parseParameters(leds_);

    	transfParameters_.resize(7);
    	
    	// initialize subscriber
        sub_phase_space_markers_ = nh_.subscribe(nh_.resolveName("/phase_space_markers"), 10, &PoseTracker::estimatePose,this);
    
    	string tansform_name =reference_frame_+"_to_"+object_frame_;
        pub_transform_= nh.advertise<geometry_msgs::TransformStamped>(nh.resolveName(tansform_name.c_str()), 10);
    }

  calc_flow_node () : nh_(), it_(nh_), flow(0, 0, CV_8UC1), prevImg(0, 0, CV_8UC1) {
    //flow = new cv::Mat(0, 0, CV_8UC1);

    namespace_ = nh_.resolveName("camera");
    camera_sub = it_.subscribeCamera(namespace_ + "/image", 10, &calc_flow_node::cameraCB, this);
    result_pub_ = nh_.advertise<sensor_msgs::Image> ("flow_image", 1);
    //result_pub_ = nh_.advertise<sensor_msgs::PointCloud2> ("flow_vector", 1);
  }

	FiducialPoseEstimator( ros::NodeHandle& nh, ros::NodeHandle& ph )
		: _fiducialManager( _lookupInterface ),
		_extrinsicsInterface( nh, ph )
	{
		GetParam<std::string>( ph, "reference_frame", _refFrame, "" );
		GetParam<std::string>( ph, "body_frame", _robotFrame, "base_link");

		bool combineDetect;
		GetParam( ph, "combine_detections", combineDetect, true);

		unsigned int inBuffSize, outBuffSize;
		GetParam( ph, "input_buffer_size", inBuffSize, (unsigned int) 20 );
		GetParam( ph, "output_buffer_size", outBuffSize, (unsigned int) 20 );

		_enableVis = ph.hasParam( "visualization" );
		if( _enableVis )
		{
			ros::NodeHandle ch( ph.resolveName( "visualization/camera" ) );
			ros::NodeHandle fh( ph.resolveName( "visualization/fiducial" ) );
			_camVis.ReadParams( ch );
			_fidVis.ReadParams( fh );

			std::string refFrame;
			GetParamRequired( ph, "visualization/reference_frame", refFrame );
			_camVis.SetFrameID( refFrame );
			_fidVis.SetFrameID( refFrame );
			_visPub = nh.advertise<MarkerMsg>( "markers", 10 );
		}

		if( combineDetect )
		{
			_detSub = nh.subscribe( "detections",
															inBuffSize,
															&FiducialPoseEstimator::DetectionsCallbackCombined,
															this );
		}
		else
		{
			_detSub = nh.subscribe( "detections",
															inBuffSize,
															&FiducialPoseEstimator::DetectionsCallbackIndependent,
															this );
		}
		_posePub = ph.advertise<geometry_msgs::TransformStamped>( "relative_pose",
		                                                          outBuffSize );
	}

    /////////////////////////////////////////////////////////////////
    // Constructor
    PedestrianDetectorHOG(ros::NodeHandle nh): 
      nh_(nh), 
      local_nh_("~"), 
      it_(nh_), 
      stereo_sync_(4),
      //      cam_model_(NULL), 
      counter(0) 
    {
      
      hog_.setSVMDetector(cv::HOGDescriptor::getDefaultPeopleDetector());
      
      // Get parameters from the server
      local_nh_.param("hit_threshold",hit_threshold_,0.0);
      local_nh_.param("group_threshold",group_threshold_,2);
      local_nh_.param("use_depth", use_depth_, true);
      local_nh_.param("use_height",use_height_,true);
      local_nh_.param("max_height_m",max_height_m_,2.2);
      local_nh_.param("ground_frame",ground_frame_,std::string("base_link"));
      local_nh_.param("do_display", do_display_, true);
       
      // Advertise a 3d position measurement for each head.
      people_pub_ = nh_.advertise<people_msgs::PositionMeasurement>("people_tracker_measurements",1);

      // Subscribe to tf & the images  
      if (use_depth_) {  

	tf_.setExtrapolationLimit(ros::Duration().fromSec(0.01));

	std::string left_topic = nh_.resolveName("stereo") + "/left/image";
	std::string disp_topic = nh_.resolveName("stereo") + "/disparity";
	std::string left_info_topic = nh_.resolveName("stereo") + "/left/camera_info";
	std::string right_info_topic = nh_.resolveName("stereo") + "/right/camera_info";
	left_sub_.subscribe(it_, left_topic, 10);
	disp_sub_.subscribe(it_, disp_topic, 10);
	left_info_sub_.subscribe(nh_, left_info_topic, 10);
	right_info_sub_.subscribe(nh_, right_info_topic, 10);
	stereo_sync_.connectInput(left_sub_, left_info_sub_, disp_sub_, right_info_sub_);
	stereo_sync_.registerCallback(boost::bind(&PedestrianDetectorHOG::imageCBAll, this, _1, _2, _3, _4));
      }
      else {
	std::string topic = nh_.resolveName("image");
	image_sub_ = it_.subscribe("image",10,&PedestrianDetectorHOG::imageCB,this);
      }

    }

 CloudPublisher()
   : tf_frame("/base_link"),
     private_nh("~")
 {
   cloud_topic = "cloud";
   pub.advertise(nh, cloud_topic.c_str(), 1);
   private_nh.param("frame_id", tf_frame, std::string("/base_link"));
   ROS_INFO_STREAM("Publishing data on topic \"" << nh.resolveName(cloud_topic) << "\" with frame_id \"" << tf_frame << "\"");
 }

  SegmentDifferencesNode  (ros::NodeHandle &n) : nh_(n)
  {
    // Maximum number of outgoing messages to be queued for delivery to subscribers = 1
    nh_.param("input_cloud_topic", input_cloud_topic_, std::string("/cloud_pcd"));
    nh_.param("output_cloud_topic", output_cloud_topic_, std::string("difference"));
    nh_.param("output_filtered_cloud_topic", output_filtered_cloud_topic_, std::string("difference_filtered"));
    nh_.param("distance_threshold", distance_threshold_, 0.0005);
    ROS_INFO ("Distance threshold set to %lf.", distance_threshold_);
    pub_diff_.advertise (nh_, output_cloud_topic_.c_str (), 1);
    ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_cloud_topic_).c_str ());
    pub_filtered_.advertise (nh_, output_filtered_cloud_topic_.c_str (), 1);
    ROS_INFO ("Publishing data on topic %s.", nh_.resolveName (output_filtered_cloud_topic_).c_str ());

    sub_.subscribe (nh_, input_cloud_topic_, 1,  boost::bind (&SegmentDifferencesNode::cloud_cb, this, _1));
    ROS_INFO ("Listening for incoming data on topic %s", nh_.resolveName (input_cloud_topic_).c_str ());
    //set PCL classes
    seg_.setDistanceThreshold (distance_threshold_);
    
    rate_ = 1;
    counter_ = 0;
  }

  StereoSynchronizer () : nh_(), it_(nh_), sync_(15) {

    std::string left_raw = nh_.resolveName("left_raw");
    std::string right_raw = nh_.resolveName("right_raw");
    image_sub_l_.subscribe(it_, left_raw  + "/image_raw", 4);
    info_sub_l_ .subscribe(nh_, left_raw  + "/camera_info", 4);
    image_sub_r_.subscribe(it_, right_raw + "/image_raw", 4);
    info_sub_r_ .subscribe(nh_, right_raw + "/camera_info", 4);

    left_ns_ = nh_.resolveName("left");
    right_ns_ = nh_.resolveName("right");
    ros::NodeHandle cam_l_nh(left_ns_);
    ros::NodeHandle cam_r_nh(right_ns_);
    it_l_ = new image_transport::ImageTransport(cam_l_nh);
    it_r_ = new image_transport::ImageTransport(cam_r_nh);
    pub_left_ = it_l_->advertiseCamera("image_raw", 1);
    pub_right_ = it_r_->advertiseCamera("image_raw", 1);

    sync_.connectInput(image_sub_l_, info_sub_l_, image_sub_r_, info_sub_r_);
    sync_.registerCallback(boost::bind(&StereoSynchronizer::imageCB, this, _1, _2, _3, _4));
  }

    /*! Also attempts to connect to database */
    Right_arm_planner(ros::NodeHandle nh) : nh_(nh), priv_nh_("~")
    {


        right_arm_planning_srv_ = nh_.advertiseService(nh_.resolveName("right_arm_planning_srv"), &Right_arm_planner::serviceCallback, this);

        right_arm_planning_controller_pub_ = nh_.advertise<control_msgs::FollowJointTrajectoryActionGoal>("/right_arm/joint_trajectory_controller/follow_joint_trajectory/goal", 1, true);
        //right_arm_planning_controller_pub_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("/right_arm/joint_trajectory_controller/follow_joint_trajectory/feedback", 1, true);

        right_arm_planning_pub_ = nh_.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);

    }

    HoughLines(ros::NodeHandle nh = ros::NodeHandle()) : it_(nh), nh_(nh), subscriber_count_(0)
    {
        image_transport::SubscriberStatusCallback connect_cb    = boost::bind(&HoughLines::connectCb, this, _1);
        image_transport::SubscriberStatusCallback disconnect_cb = boost::bind(&HoughLines::disconnectCb, this, _1);
        img_pub_ = image_transport::ImageTransport(ros::NodeHandle(nh_, "hough")).advertise("image", 1, connect_cb, disconnect_cb);

        out_pub_ = nh.advertise<jsk_perception::LineArray>(nh_.resolveName("lines"), 1);


        dynamic_reconfigure::Server<jsk_perception::HoughLinesConfig>::CallbackType f =
            boost::bind(&HoughLines::reconfigureCallback, this, _1, _2);
        srv.setCallback(f);
    }

void DiscretePolicyManager::Initialize( DiscretePolicyInterface::Ptr& interface,
                                        ros::NodeHandle& nh,
                                        ros::NodeHandle& ph )
{
	_interface = interface;

	_actionPub = ph.advertise<DiscreteAction>( "actions", 0 );
	_paramSub = nh.subscribe( "param_updates", 
	                          1,
	                          &DiscretePolicyManager::ParamCallback, 
	                          this );

	ros::NodeHandle subh( ph.resolveName("input_streams") );
	_inputStreams.Initialize( subh );

	unsigned int inputDim = _inputStreams.GetDim();
	unsigned int outputDim = _interface->GetOutputSizes().array().prod();
	unsigned int numHiddenLayers, layerWidth;
	GetParamRequired( ph, "network/num_hidden_layers", numHiddenLayers );
	GetParamRequired( ph, "network/layer_width", layerWidth );
	_network = std::make_shared<NetworkType>( inputDim, 
	                                          outputDim, 
	                                          numHiddenLayers, 
	                                          layerWidth );
	_network->SetInputSource( &_networkInput );
	_networkParameters = _network->CreateParameters();
	VectorType w = _networkParameters->GetParamsVec();
	percepto::randomize_vector( w, -0.1, 0.1 );
	_networkParameters->SetParamsVec( w );

	if( HasParam( ph, "seed" ) )
	{
		int seed;
		GetParam( ph, "seed", seed );
		_engine.seed( seed );
	}
	else
	{
		boost::random::random_device rng;
		_engine.seed( rng() );
	}

	double updateRate;
	GetParamRequired( ph, "update_rate", updateRate );
	_timer = ph.createTimer( ros::Duration( 1.0/updateRate ),
	                         &DiscretePolicyManager::UpdateCallback,
	                         this );
}

  bool spin ()
  {
    int nr_points = cloud.width * cloud.height;
    std::string fields_list = pcl::getFieldsList(cloud);
    ros::Rate r(ros::Duration(1,0)); //1s tact

    while(nh.ok ())
    {
      ROS_DEBUG_STREAM_ONCE("Publishing data with " << nr_points
                            << " points " << fields_list
                            << " on topic \"" << nh.resolveName(cloud_topic)
                            << "\" in frame \"" << cloud.header.frame_id << "\"");
      cloud.header.stamp = ros::Time::now();
      pub.publish(cloud);

      r.sleep();
    }
    return (true);
  }