C++ Subscriber::getNumPublishers方法代码示例

//MAIN
int main(int argc, char** argv)
{

	ros::init(argc, argv, "wheel_motor_node"); //Initialize node

	ros::NodeHandle n; //Create nodehandle object

	sub = n.subscribe("wheel_motor_rc", 1000, callBack); //Create object to subscribe to topic "wheel_motor_rc"
	
	pub = n.advertise<motor_controller::AdaCmd>("adaFruit",1000); //Create object to publish to topic "I2C"
	while(pub.getNumSubscribers()==0);//Prevents message from sending when publisher is not completely connected to subscriber.
	
	//ros::Rate loop_rate(10); //Set frequency of looping. 10 Hz
	
	setGPIOWrite(33,1); //Motor enable

	while(ros::ok())
	{
		//Update time
		current_time = ros::Time::now();
		//Check if interval has passed
		if(current_time - last_time > update_rate)
		{
			//Reset time
			last_time = current_time;
			if(sub.getNumPublishers() == 0) //In case of loss of connection to publisher, set controller outputs to 0
			{
				ROS_WARN("Loss of wheel motor controller input!");
				leftFrontWheel.setU(0); //Set controller inputs
				leftRearWheel.setU(0);
				rightRearWheel.setU(0);
				rightFrontWheel.setU(0);
			}

			controlFunction(); //Motor controller function
			pub.publish(generateMessage());
		}
		
			ros::spinOnce();
	}


	stopAllMotors();

	return 0;
}

		MyLittleTurtle(ros::NodeHandle& nodeHandle): m_nodeHandle(nodeHandle)
		{
			ROS_INFO("Publishing topic...");
			m_turtlesimPublisher = m_nodeHandle.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
			
			ROS_INFO("Subscribing to turtlesim topic...");
			m_turtlesimSubscriber = m_nodeHandle.subscribe("/turtle1/pose", 10, &MyLittleTurtle::updateTurtleStatus, this);

			ROS_INFO("Waiting for turtlesim...");
			ros::Rate loopRate(10);
			while (ros::ok() && (m_turtlesimPublisher.getNumSubscribers() <= 0 || m_turtlesimSubscriber.getNumPublishers() <= 0))
				loopRate.sleep();
			checkRosOk_v();
			
			ROS_INFO("Retrieving initial status...");
			ros::spinOnce();
			ROS_INFO("Initial status: x=%.3f, y=%.3f, z=%.3f", m_status.x, m_status.y, m_status.z);
			
			ROS_INFO("Everything's ready.");
		}

    // move platform server receives a new goal
    void moveGoalCB()
    {
        set_terminal_state_ = true;

        move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
        ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);

        if (as_.isPreemptRequested() ||!ros::ok())
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
            if (planning_)
                ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            if (controlling_)
                ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
            move_result_.result_state = 0;
            move_result_.error_string = "Preempt Requested!!!";
            as_.setPreempted(move_result_);
            return;
        }

        // Convert move goal to plan goal
        pose_goal_.pose_goal = move_goal_.nav_goal;

        if (planner_state_sub.getNumPublishers()==0)
        {
            ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
            planning_ = false;
            move_result_.result_state = 0;
            move_result_.error_string = "Planner is down";
            as_.setAborted(move_result_);
        }
        else
        {
            ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
                                 actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
            planning_ = true;
            ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
        }
        return;
    }

    void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
    {
        planning_ = false;
        ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());

        move_result_.result_state = result->result_state;

        if (move_result_.result_state) //if plan OK
        {
            planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner

            if (ctrl_state_sub.getNumPublishers()==0)
            {
                ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
                controlling_ = false;
                move_result_.result_state = 0;
                move_result_.error_string = "Controller is down!!!";
                as_.setAborted(move_result_);
            }
            else
            {
                ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
                                     actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
                                     actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
                controlling_ = true;
                ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
            }
        }
        else //if plan NOT OK
        {
            ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());

            move_result_.error_string = "Planning Failed: " + result->error_string;
            ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);

            as_.setAborted(move_result_);
        }
        return;

    }

 bool IsNodeReady()
 {
     return (pub.getNumSubscribers() > 0) && (sub.getNumPublishers() > 0);
 }

    //publish state timer
    void state_pub_timerCB(const ros::TimerEvent& event)
    {
        if (ctrl_state_sub.getNumPublishers()==0)
        {
            hw_state_.state = hardware::ERROR;
            hw_state_.description = "Platform motion controller is down" ;
            state_pub_.publish(hw_state_);
            return;
        }

        if (planner_state_sub.getNumPublishers()==0)
        {
            hw_state_.state = hardware::ERROR;
            hw_state_.description = "Platform motion planner is down" ;
            state_pub_.publish(hw_state_);
            return;
        }

        if (plan_state_ == hardware::BUSY)
        {
            hw_state_.state = hardware::BUSY;
            hw_state_.description = "Robot is planning" ;
        }
        else
        {
            switch (control_state_)
            {
            case 0:
                hw_state_.state = hardware::IDLE;
                hw_state_.description = "Ready to receive a goal" ; // - What is zero?
                break;
            case 1:
                hw_state_.state = hardware::BUSY;
                hw_state_.description = "Robot is moving at normal speed" ;
                break;
            case 2:
                hw_state_.state = hardware::BUSY;
                hw_state_.description = "Robot is moving at slow speed" ;
                break;
            case 5:
                hw_state_.state = hardware::IDLE;
                hw_state_.description = "Ready to receive a goal" ;
                break;
            case 10:
                hw_state_.state = hardware::ERROR;
                hw_state_.description = "Robot is in EMERGENCY state" ;
                break;
            case 20:
                hw_state_.state = hardware::BUSY;
                hw_state_.description = "Some obstacle in the way of the robot" ;
                break;
            case 21:
                hw_state_.state = hardware::ERROR;
                hw_state_.description = "Immovable obstacle in the way of the robot" ;
                break;
            case 30:
                hw_state_.state = hardware::ERROR;
                hw_state_.description = "Localization NOT OK" ;
                break;
            }
        }

        state_pub_.publish(hw_state_);
        return;
    }