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

CoaxSimpleControl::CoaxSimpleControl(ros::NodeHandle &node)
    :reach_nav_state(node.serviceClient<coax_msgs::CoaxReachNavState>("reach_nav_state"))
    ,configure_comm(node.serviceClient<coax_msgs::CoaxConfigureComm>("configure_comm"))
    ,configure_control(node.serviceClient<coax_msgs::CoaxConfigureControl>("configure_control"))
    ,set_timeout(node.serviceClient<coax_msgs::CoaxSetTimeout>("set_timeout"))

    ,coax_state_sub(node.subscribe("state",1, &CoaxSimpleControl::coaxStateCallback, this))
    ,coax_tf_sub(node.subscribe("tf",1, &CoaxSimpleControl::coaxTfCallback, this))

    ,raw_control_pub(node.advertise<coax_msgs::CoaxRawControl>("rawcontrol",1))
    ,simple_control_pub(node.advertise<coax_msgs::CoaxControl>("simplecontrol",1))

    ,LOW_POWER_DETECTED(false)
    ,CONTROL_MODE(CONTROL_LANDED)
    ,FIRST_START(false)
    ,FIRST_LANDING(false)
    ,FIRST_HOVER(false)
    ,INIT_DESIRE(false)
    ,coax_nav_mode(0)
    ,coax_control_mode(0)
    ,coax_state_age(0)
    ,raw_control_age(0)
    ,init_count(0)
    ,mil_count(0)
    ,rotor_ready_count(-1)
    ,battery_voltage(12.22)
    ,imu_y(0.0),imu_r(0.0),imu_p(0.0)
    ,range_al(0.0)
    ,rc_th(0.0),rc_y(0.0),rc_r(0.0),rc_p(0.0)
    ,rc_trim_th(0.0),rc_trim_y(0.104),rc_trim_r(0.054),rc_trim_p(0.036)
    ,img_th(0.0),img_y(0.0),img_r(0.0),img_p(0.0)
    ,gyro_ch1(0.0),gyro_ch2(0.0),gyro_ch3(0.0)
    ,accel_x(0.0),accel_y(0.0),accel_z(0.0)
    ,mag_1(0.0), mag_2(0.0), mag_3(0.0)
    ,motor_up(0),motor_lo(0)
    ,servo_roll(0),servo_pitch(0)
    ,roll_trim(0),pitch_trim(0)
    ,motor1_des(0.0),motor2_des(0.0),servo1_des(0.0),servo2_des(0.0)
    ,yaw_des(0.0),yaw_rate_des(0.0)
    ,roll_des(0.0),roll_rate_des(0.0)
    ,pitch_des(0.0),pitch_rate_des(0.0)
    ,altitude_des(1.5), coax_global_x(0.0), coax_global_y(0.0), coax_global_z(0.0)
    ,qnd0(0.0), qnd1(0.0), qnd2(0.0), qnd3(0.0)
    ,Gx_des(0.0), Gy_des(0.0)
    ,PI(3.14159265), r2d(57.2958), d2r(0.0175)
    ,ang_err_lim(0.14)
    ,x_vel_hist_1(0.0), x_vel_hist_2(0.0), x_vel_hist_3(0.0) , x_vel_hist_4(0.0)
    ,y_vel_hist_1(0.0), y_vel_hist_2(0.0), y_vel_hist_3(0.0) , y_vel_hist_4(0.0)
    ,x_gyro_1(0.0), x_gyro_2(0.0)
    ,y_gyro_1(0.0), y_gyro_2(0.0)
    ,Gz_old1(0.0), Gz_old2(0.0), Gz_old3(0.0)
    ,way_changed(0)
    ,way_old{0.0, 0.0, 0.0}

{
    set_nav_mode.push_back(node.advertiseService("set_nav_mode", &CoaxSimpleControl::setNavMode, this));
    set_control_mode.push_back(node.advertiseService("set_control_mode", &CoaxSimpleControl::setControlMode, this));
    set_waypoint.push_back(node.advertiseService("set_waypoint", &CoaxSimpleControl::setWaypoint, this));
    loadParams(node);
}

  PeoplePositionServer()
  {
    rein_sub_ 
      = n.subscribe("/humans/RecogInfo", 1, 
		    &PeoplePositionServer::callback, this);
    face_now_pub_
      = n.advertise<humans_msgs::PersonPoseImgArray
		    >("/now_human", 10);

    track_srv_ 
      = n.advertiseService("track_srv", 
			  &PeoplePositionServer::resTrackingId, this);
    okao_srv_ 
      = n.advertiseService("okao_srv", 
			   &PeoplePositionServer::resOkaoId, this);

     name_srv_ 
       = n.advertiseService("name_srv", 
			    &PeoplePositionServer::resName, this);
     
    okaoStack_
      = n.serviceClient<okao_client::OkaoStack>("stack_send");

    //array_srv_ 
    // = n.advertiseService("array_srv", 
    //			   &PeoplePositionServer::resHumans, this);

    
  }

//! Initialize plugin - called in server constructor
void srs_env_model::CCMapPlugin::init(ros::NodeHandle & node_handle)
{
	m_collisionMapVersion = 0;
	m_dataBuffer->boxes.clear();
	m_data->boxes.clear();

	// Read parameters

	// Get collision map radius
	node_handle.param("collision_map_radius", m_collisionMapLimitRadius, COLLISION_MAP_RADIUS_LIMIT );

	// Get collision map topic name
	node_handle.param("collision_map_publisher", m_cmapPublisherName, COLLISION_MAP_PUBLISHER_NAME );

	// Get FID to which will be points transformed when publishing collision map
	node_handle.param("collision_map_frame_id", m_cmapFrameId, COLLISION_MAP_FRAME_ID ); //

	// Create and publish service - get collision map
	m_serviceGetCollisionMap = node_handle.advertiseService( GetCollisionMap_SRV, 	&CCMapPlugin::getCollisionMapSrvCallback, this);

	// Create and publish service - is new collision map
	m_serviceIsNewCMap = node_handle.advertiseService( IsNewCMap_SRV, &CCMapPlugin::isNewCmapSrvCallback, this );

	// Create and publish service - lock map
	m_serviceLockCMap = node_handle.advertiseService( LockCMap_SRV, &CCMapPlugin::lockCmapSrvCallback, this );

	// Create and publish service - remove cube from map
	m_serviceRemoveCube = node_handle.advertiseService( RemoveCubeCMP_SRV, &CCMapPlugin::removeBoxCallback, this );

	// Create and publish service - add cube to map
	m_serviceAddCube = node_handle.advertiseService( AddCubeCMP_SRV, &CCMapPlugin::addBoxCallback, this );

	// Connect publishing services
	pause( false, node_handle );
}

    SnapshotMap( ros::NodeHandle& nh ) : nh_( nh ),object_available(false) {

		imageAllocator_ = boost::shared_ptr< MultiResolutionSurfelMap::ImagePreAllocator >( new MultiResolutionSurfelMap::ImagePreAllocator() );
		treeNodeAllocator_ = boost::shared_ptr< spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue > >( new spatialaggregate::OcTreeNodeDynamicAllocator< float, MultiResolutionSurfelMap::NodeValue >( 10000 ) );

		snapshot_service_ = nh.advertiseService("snapshot", &SnapshotMap::snapshotRequest, this);
		object_pose_service_ = nh.advertiseService("object_pose", &SnapshotMap::poseRequest, this);
		pub_cloud = pcl_ros::Publisher<pcl::PointXYZRGB>(nh, "output_cloud", 1);

		pub_status_ = nh.advertise< std_msgs::Int32 >( "status", 1 );

		tf_listener_ = boost::shared_ptr< tf::TransformListener >( new tf::TransformListener() );

		//added to debug
		m_debugObjPub = nh.advertise<geometry_msgs::PoseStamped>(
					"debug_object", 1, true);

		nh.param<double>( "max_resolution", max_resolution_, 0.0125 );
		nh.param<double>( "max_radius", max_radius_, 30.0 );

		nh.param<double>( "dist_dep_factor", dist_dep_, 0.005 );

		nh.param<std::string>( "map_folder", map_folder_, "." );

		nh.param<std::string>( "init_frame", init_frame_, "" );

		create_map_ = false;
		do_publish_tf_= true;

		responseId_ = -1;

    }

TaskScheduler::TaskScheduler(ros::NodeHandle & nh, boost::shared_ptr<TaskDefinition> tidle, double deftPeriod)
{
	aqid = 0;
	runScheduler = false;

	idle = tidle;
	idleTimeout = 0.001;

	tasks["idle"] = idle;
	defaultPeriod = deftPeriod;
	startingTime = ros::Time::now().toSec();

	mainThread.reset();
	pthread_mutex_init(&scheduler_mutex,NULL);
	pthread_cond_init(&scheduler_condition,NULL);
	pthread_mutex_init(&aqMutex,NULL);
	pthread_cond_init(&aqCond,NULL);

	ROS_INFO("Task scheduler created: debug %d\n",debug);

    n = nh;
    startTaskSrv = nh.advertiseService("start_task", &TaskScheduler::startTask,this);
    stopTaskSrv = nh.advertiseService("stop_task", &TaskScheduler::stopTask,this);
    getTaskListSrv = nh.advertiseService("get_all_tasks", &TaskScheduler::getTaskList,this);
    getTaskListLightSrv =nh.advertiseService("get_all_tasks_light", &TaskScheduler::getTaskListLight,this);
    getAllTaskStatusSrv = nh.advertiseService("get_all_status", &TaskScheduler::getAllTaskStatus,this);
    statusPub = nh.advertise<task_manager_msgs::TaskStatus>("status",20);
    keepAliveSub = nh.subscribe("keep_alive",1,&TaskScheduler::keepAliveCallback,this);
    lastKeepAlive = ros::Time::now();
}

        // Constructor
        NodeClass()
        {
			// initialization of variables
			is_initialized_bool_ = false;
			last_time_ = ros::Time::now();
			x_rob_m_ = 0.0;
			y_rob_m_ = 0.0;
			theta_rob_rad_ = 0.0;
			// set status of drive chain to WARN by default
			drive_chain_diagnostic_ = diagnostic_status_lookup_.WARN;
			
			// implementation of topics
            // published topics
			topic_pub_joint_state_cmd_ = n.advertise<sensor_msgs::JointState>("JointStateCmd", 1);
			topic_pub_odometry_ = n.advertise<nav_msgs::Odometry>("Odometry", 50);

            // subscribed topics
			topic_sub_CMD_pltf_twist_ = n.subscribe("cmd_vel", 1, &NodeClass::topicCallbackTwistCmd, this);
            topic_sub_EM_stop_state_ = n.subscribe("EMStopState", 1, &NodeClass::topicCallbackEMStop, this);
            topic_sub_drive_diagnostic_ = n.subscribe("Diagnostic", 1, &NodeClass::topicCallbackDiagnostic, this);
			//<diagnostic_msgs::DiagnosticStatus>("Diagnostic", 1);

            // implementation of service servers
            srv_server_init_ = n.advertiseService("Init", &NodeClass::srvCallbackInit, this);
            srv_server_reset_ = n.advertiseService("Reset", &NodeClass::srvCallbackReset, this);
            srv_server_shutdown_ = n.advertiseService("Shutdown", &NodeClass::srvCallbackShutdown, this);

			// implementation of service clients
            srv_client_get_joint_state_ = n.serviceClient<cob_srvs::GetJointState>("GetJointState");
        }

  FootstepPlanner::FootstepPlanner(ros::NodeHandle& nh):
    as_(nh, nh.getNamespace(),
        boost::bind(&FootstepPlanner::planCB, this, _1), false)
  {
    srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (nh);
    typename dynamic_reconfigure::Server<Config>::CallbackType f =
      boost::bind (&FootstepPlanner::configCallback, this, _1, _2);
    srv_->setCallback (f);
    pub_text_ = nh.advertise<jsk_rviz_plugins::OverlayText>(
      "text", 1, true);
    pub_close_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "close_list", 1, true);
    pub_open_list_ = nh.advertise<sensor_msgs::PointCloud2>(
      "open_list", 1, true);
    srv_project_footprint_ = nh.advertiseService(
      "project_footprint", &FootstepPlanner::projectFootPrintService, this);
    srv_project_footprint_with_local_search_ = nh.advertiseService(
      "project_footprint_with_local_search", &FootstepPlanner::projectFootPrintWithLocalSearchService, this);
    {
      boost::mutex::scoped_lock lock(mutex_);
      if (!readSuccessors(nh)) {
        return;
      }

      JSK_ROS_INFO("building graph");
      buildGraph();
      JSK_ROS_INFO("build graph done");
    }
    sub_pointcloud_model_ = nh.subscribe("pointcloud_model", 1, &FootstepPlanner::pointcloudCallback, this);
    as_.start();
  }

  ///Constructor
  PowerCubeChainNode()
  {
  	n_ = ros::NodeHandle("~");

    pc_params_ = new PowerCubeCtrlParams();
    pc_ctrl_ = new PowerCubeCtrl(pc_params_);

    /// implementation of topics to publish
    topicPub_JointState_ = n_.advertise<sensor_msgs::JointState> ("/joint_states", 1);
    topicPub_ControllerState_ = n_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState> ("state", 1);
    topicPub_OperationMode_ = n_.advertise<std_msgs::String> ("current_operationmode", 1);
    topicPub_Diagnostic_ = n_.advertise<diagnostic_msgs::DiagnosticArray>("/diagnostics", 1);

    /// implementation of topics to subscribe
    topicSub_CommandPos_ = n_.subscribe("command_pos", 1, &PowerCubeChainNode::topicCallback_CommandPos, this);
    topicSub_CommandVel_ = n_.subscribe("command_vel", 1, &PowerCubeChainNode::topicCallback_CommandVel, this);

    /// implementation of service servers
    srvServer_Init_ = n_.advertiseService("init", &PowerCubeChainNode::srvCallback_Init, this);
    srvServer_Stop_ = n_.advertiseService("stop", &PowerCubeChainNode::srvCallback_Stop, this);
    srvServer_Recover_ = n_.advertiseService("recover", &PowerCubeChainNode::srvCallback_Recover, this);
    srvServer_SetOperationMode_ = n_.advertiseService("set_operation_mode", &PowerCubeChainNode::srvCallback_SetOperationMode, this);

    initialized_ = false;
    stopped_ = true;
    error_ = false;
    last_publish_time_ = ros::Time::now();
  }

CloudMatcher::CloudMatcher(ros::NodeHandle& n):
	n(n),
	serviceMatchClouds(n.advertiseService("match_clouds", &CloudMatcher::match, this)),
	serviceMatchCloudsWithGuess(n.advertiseService("match_clouds_with_guess", &CloudMatcher::matchWithGuess, this))
{
	// load config
	string configFileName;
	if (ros::param::get("~config", configFileName))
	{
		ifstream ifs(configFileName.c_str());
		if (ifs.good())
		{
			icp.loadFromYaml(ifs);
		}
		else
		{
			ROS_ERROR_STREAM("Cannot load config from YAML file " << configFileName);
			icp.setDefault();
		}
	}
	else
	{
		ROS_WARN_STREAM("No config file specified, using default ICP chain.");
		icp.setDefault();
	}
	
	// replace logger
	if (getParam<bool>("useROSLogger", false))
		PointMatcherSupport::setLogger(new PointMatcherSupport::ROSLogger);
}

EventsGenerator::EventsGenerator(ros::NodeHandle& n, ros::NodeHandle& np)
{
	double p;
	np.param("exp_timeout", p, 0.5);
	exp_timeout_ = ros::Duration(p);

	sub_desires_ = n.subscribe("desires_set", 1,
			&EventsGenerator::desiresCB, this);
	sub_intention_ = n.subscribe("intention", 1,
			&EventsGenerator::intentionCB, this);
	srv_cem_ = n.advertiseService("create_exploitation_matcher",
			&EventsGenerator::cemCB, this);
	srv_ctem_ = n.advertiseService("create_topic_exploitation_matcher",
			&EventsGenerator::ctemCB, this);

	pub_events_ = n.advertise<hbba_msgs::Event>("events", 100);

	ros::NodeHandle rosgraph_np(np, "rosgraph_monitor");
	rosgraph_monitor_.reset(new RosgraphMonitor(n, rosgraph_np));
	rosgraph_monitor_->registerCB(&EventsGenerator::rosgraphEventsCB, this);

	timer_ = n.createTimer(ros::Duration(1.0), &EventsGenerator::timerCB, this);

    parseExploitationMatches(n);
}

	CaptureServer() :
			nh_private("~") {

		ROS_INFO("Creating localization");

		queue_size_ = 5;

		odom_pub = nh_.advertise<nav_msgs::Odometry>("vo", queue_size_);
		keyframe_pub = nh_.advertise<rm_localization::Keyframe>("keyframe",
				queue_size_);

		update_map_service = nh_.advertiseService("update_map",
				&CaptureServer::update_map, this);

		send_all_keyframes_service = nh_.advertiseService("send_all_keyframes",
				&CaptureServer::send_all_keyframes, this);

		clear_keyframes_service = nh_.advertiseService("clear_keyframes",
				&CaptureServer::clear_keyframes, this);

		rgb_sub.subscribe(nh_, "rgb/image_raw", queue_size_);
		depth_sub.subscribe(nh_, "depth/image_raw", queue_size_);
		info_sub.subscribe(nh_, "rgb/camera_info", queue_size_);

		// Synchronize inputs.
		sync.reset(
				new Synchronizer(SyncPolicy(queue_size_), rgb_sub, depth_sub,
						info_sub));

		sync->registerCallback(
				boost::bind(&CaptureServer::RGBDCallback, this, _1, _2, _3));

	}

  ShooterVision() : nh_(ros::this_node::getName()), it_(nh_) {
    nh_.param<bool>("auto_start", active, false);
    vision.reset(new GrayscaleContour(nh_));
    vision->init();
    nh_.param<std::string>("symbol_camera", camera_topic,
                           "/right/right/image_raw");
    std::string get_shapes_topic;
    nh_.param<std::string>("get_shapes_topic",get_shapes_topic,"get_shapes");
    runService = nh_.advertiseService(get_shapes_topic+"/switch", &ShooterVision::runCallback, this);
    roiService = nh_.advertiseService("setROI",
                                      &ShooterVision::roiServiceCallback, this);
    //#ifdef DO_DEBUG
    // DebugWindow::init();
    //#endif
    foundShapesPublisher = nh_.advertise<navigator_msgs::DockShapes>(
        "found_shapes", 1000);
    image_sub_ = it_.subscribe(camera_topic, 1, &ShooterVision::run, this);

    int x_offset, y_offset, width, height;
    nh_.param<int>("roi/x_offset", x_offset, 73);
    nh_.param<int>("roi/y_offset", y_offset, 103);
    nh_.param<int>("roi/width", width, 499);
    nh_.param<int>("roi/height", height, 243);
    nh_.param<int>("size/height", size.height, 243);
    nh_.param<int>("size/width", size.width, 243);
    roi = Rect(x_offset, y_offset, width, height);
    TrackedShape::init(nh_);
    tracker.init(nh_);
  }

bool ExcavaROBArmKinematics::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("ExcavaROB Kinematics: Reading xml file from parameter server");
    std::string result;
    if (!nh_.getParam(full_urdf_xml, result)) {
        ROS_FATAL("ExcavaROB Kinematics: Could not load the xml from parameter server: %s", urdf_xml.c_str());
	return false;
    }

    // Get Root, Wrist and Tip From Parameter Service
    if (!nh_private_.getParam("root_name", root_name_)) {
        ROS_FATAL("ExcavaROB Kinematics: No root_name found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("tip_name", tip_name_)) {
        ROS_FATAL("ExcavaROB Kinematics: No tip_name found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("max_iterations", max_iterations_)) {
        ROS_FATAL("ExcavaROB Kinematics: No max_iterations found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("eps", eps_)) {
        ROS_FATAL("ExcavaROB Kinematics: No eps found on parameter server");
        return false;
    }
    if (!nh_private_.getParam("ik_gain", ik_gain_)) {
        ROS_FATAL("ExcavaROB Kinematics: No ik_gain 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);

    // Build Solvers
    fk_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);
    jnt_to_jac_solver_ = new KDL::ChainJntToJacSolver(arm_chain_);
    jnt_to_pose_solver_ = new KDL::ChainFkSolverPos_recursive(arm_chain_);

    ROS_INFO("ExcavaROB Kinematics: Advertising services");
    ik_service_ = nh_private_.advertiseService(IK_SERVICE, &ExcavaROBArmKinematics::getPositionIK, this);
    fk_service_ = nh_private_.advertiseService(FK_SERVICE, &ExcavaROBArmKinematics::getPositionFK, this);

    kin_solver_info_service_ = nh_private_.advertiseService(KIN_INFO_SERVICE, &ExcavaROBArmKinematics::getKinSolverInfo, this);

    return true;
}

 void init()
 {
   nh.initNode();
   //~ nh.subscribe(sub);
   nh.advertiseService(fireService);
   nh.advertiseService(cancelService);
   nh.advertiseService(manualService);
 }

T11::T11(ros::NodeHandle node) {
  knowledge_pub = node.advertise<t11_kb_modeling::Knowledge>(TOPIC_KB, 100);
  position_sub = node.subscribe(TOPIC_ROBOT_LOCATION, 100, &T11::positionCallback, this);
  hri_feature_sub = node.subscribe("t31_feature", 10, &T11::hriFeatureCallback, this);
  env_feature_sub = node.subscribe("t21_feature", 10, &T11::envFeatureCallback, this);
  service_get_location = node.advertiseService(SERVICE_GET_LOCATION, &T11::getLocation, this);
  service_get_all_sites = node.advertiseService(SERVICE_GET_ALL_SITES, &T11::getAllSites, this);
}