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

	DataProcessor::DataProcessor(ros::NodeHandle & n): n_(n) 
	{
		processed_pointcloud_ = false;
		new_cloud_wanted_ = false;
		nocluster_count_ = 0;
		process_service_ = n_.advertiseService("process_pcd", &DataProcessor::processDataCallback, this);
		pub_active_region_ = n_.advertise<sensor_msgs::PointCloud2>("activeregion",1);
		marker_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 1);
		regions_pub_ = n_.advertise<visualization_msgs::MarkerArray>("region_nums", 1);

		//ros::Subscriber sub = n.subscribe("/camera/rgb/points",1,got_point_cloud);
		//client_grasp_ = n_.serviceClient<duplo_v1::Grasp_Duplo>("grasp_duplo");
		client_manipulate_ = n_.serviceClient<duplo_v1::Manipulate_Duplo>("manipulate_duplo");
		//client_tumble_ = n_.serviceClient<duplo_v1::Tumble_Duplo>("tumble_duplo");
		reset_posn.x = 0.6;	reset_posn.y = -0.5; reset_posn.z = 1;

		while ( !ros::service::waitForService("get_new_pcd",ros::Duration(2.0)) && n.ok() ) 
			ROS_INFO("DUPLO: Waiting for object detection service to come up");
		if (!n.ok()) exit(0);

		pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp1(new pcl::PointCloud<pcl::PointXYZRGB>);
		object_cloud_ = temp1;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr temp2(new pcl::PointCloud<pcl::PointXYZRGB>);
		planar_cloud_ = temp2;
		
		ROS_INFO("Ready to process input point cloud data.");
		client_newpcd_ = n.serviceClient<duplo_v1::Get_New_PCD>("get_new_pcd");
	}

  TFMonitor(bool using_specific_chain, std::string framea  = "", std::string frameb = ""):
    framea_(framea), frameb_(frameb),
    using_specific_chain_(using_specific_chain)
  {
    
    if (using_specific_chain_)
    {
      cout << "Waiting for transform chain to become available between "<< framea_ << " and " << frameb_<< " " << flush;
      while (node_.ok() && !tf_.waitForTransform(framea_, frameb_, Time(), Duration(1.0)))
        cout << "." << flush;
      cout << endl;
     
      try{
        tf_.chainAsVector(frameb_, ros::Time(), framea_, ros::Time(), frameb_, chain_);
      }
      catch(tf::TransformException& ex){
        ROS_WARN("Transform Exception %s", ex.what());
        return;
      } 

      /*      cout << "Chain currently is:" <<endl;
      for (unsigned int i = 0; i < chain_.size(); i++)
      {
        cout << chain_[i] <<", ";
      }
      cout <<endl;*/
    }
    subscriber_tf_ = node_.subscribe<tf::tfMessage>("tf", 100, boost::bind(&TFMonitor::callback, this, _1));
    subscriber_tf_message_ = node_.subscribe<tf::tfMessage>("tf_message", 100, boost::bind(&TFMonitor::callback, this, _1));
    
  }

bool spin()
{
    ROS_INFO("summit_robot_control::spin()");
    ros::Rate r(desired_freq_);  // 50.0 

    while (!ros::isShuttingDown()) // Using ros::isShuttingDown to avoid restarting the node during a shutdown.
    {
      if (starting() == 0)
      {
	    while(ros::ok() && node_handle_.ok()) {
          UpdateControl();
          UpdateOdometry();
          PublishOdometry();
          diagnostic_.update();
          ros::spinOnce();
	      r.sleep();
          }
	      ROS_INFO("END OF ros::ok() !!!");
      } else {
       // No need for diagnostic here since a broadcast occurs in start
       // when there is an error.
       usleep(1000000);
       ros::spinOnce();
      }
   }

   return true;
}

 void Spin(){
   while(nh.ok()){
     Receive();
     ros::spinOnce();
     if (step(TIME_STEP) == -1) break;
   }
 }

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

    geometry_msgs::PoseStamped getCartBaseLinkPosition()
    {
        geometry_msgs::PoseStamped pose_base_link;
        pose_base_link.header.frame_id = base_link_;

        while (nh_.ok()) {
            try {
                tf_listener_.lookupTransform(base_link_, lwr_tip_link_, ros::Time(0), base_link_transform_);

                pose_base_link.pose.position.x = base_link_transform_.getOrigin().x();
                pose_base_link.pose.position.y = base_link_transform_.getOrigin().y();
                pose_base_link.pose.position.z = base_link_transform_.getOrigin().z();
                pose_base_link.pose.orientation.x = base_link_transform_.getRotation().x();
                pose_base_link.pose.orientation.y = base_link_transform_.getRotation().y();
                pose_base_link.pose.orientation.z = base_link_transform_.getRotation().z();
                pose_base_link.pose.orientation.w = base_link_transform_.getRotation().w();

                return pose_base_link;
            }
            catch (tf::TransformException ex){
                ROS_ERROR("%s",ex.what());
                ros::Duration(0.1).sleep();
            }
        }
    }

 void
 run ()
 {
   while (nh_.ok ())
   {
     ros::spinOnce ();
   }
 }

void LoopbackControllerManager::run()
{

  ros::Rate rate(1.0/dt_);
  while(rosnode_->ok())
  {
    update();
    rate.sleep();
  }
}

  //Close the gripper
  void close(){
    
    ROS_INFO("Closing Gripper");
    ros::Duration(0.5).sleep();

    //wait for the listener to get the first message
    listener_.waitForTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                               ros::Time(0), ros::Duration(1.0));
    
    //we will record transforms here
    tf::StampedTransform start_transform;
    tf::StampedTransform current_transform;

    //record the starting transform from the gripper to the base frame
    listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                              ros::Time(0), start_transform);

    bool done = false;
    pr2_controllers_msgs::Pr2GripperCommand gripper_cmd;
    gripper_cmd.position = 0.0;
    gripper_cmd.max_effort = 50.0;

    ros::Rate rate(10.0);

    double dist_moved_before;
    double dist_moved;
    while (!done && nh_.ok())
    {
      r_gripper_.publish(gripper_cmd);

      rate.sleep();
      //get the current transform
      try
      {
        listener_.lookupTransform("base_footprint", "r_gripper_l_finger_tip_frame", 
                                  ros::Time(0), current_transform);
      }
      catch (tf::TransformException ex)
      {
        ROS_ERROR("%s",ex.what());
        break;
      }
  
      //see how if the gripper is open or if it hit some object
      tf::Transform relative_transform = 
        start_transform.inverse() * current_transform;

      dist_moved_before = dist_moved;
      dist_moved = relative_transform.getOrigin().length();

      //ROS_INFO("%f",dist_moved);
      if(dist_moved > 0.03 || dist_moved < dist_moved_before) done = true;
    
    }
  }

void I2cImu::spin()
{
	ros::Rate r(1.0 / (imu_->IMUGetPollInterval() / 1000.0));
	while (nh_.ok())
	{
		update();

		ros::spinOnce();
		r.sleep();
	}
}

  int main (int argc, char **argv) {
    struct usb_dev_handle *dev;
  
    dev = ctxInit();
    
    if (!dev) {
      perror("Initializing CNX-P2140");
      return -1;
    }

    ros::NodeHandle node_param("~");
    node_param.param<double>("diag_frequency", diag_frequency, 1.0);
    node_param.param<double>("input", input_nominal, 13.8);
    node_param.param<double>("primary", primary, 18.5);
    node_param.param<double>("secondary", secondary, 12.0);

    diag_pub = node.advertise<power_msgs::PowerReading>("ctx2140", 1);

    ros::Rate loop_rate(diag_frequency);
    while (node.ok()) {
      ctxValues diag;
      power_msgs::PowerReading reading;
      
      if (ctxReadValues(dev, &diag)) {
	printf("Error reading from ctx2140\n");
	return -1;
      }

      reading.volts_read.push_back(diag.battVoltage);
      reading.volts_read.push_back(diag.priVoltage);
      reading.volts_read.push_back(diag.secVoltage);

      reading.current.push_back(diag.battCurrent);
      reading.current.push_back(diag.priCurrent);
      reading.current.push_back(diag.secCurrent);

      reading.volts_full.push_back(input_nominal);
      reading.volts_full.push_back(primary);
      reading.volts_full.push_back(secondary);

      reading.temperature.push_back(diag.temperature);

      reading.header.stamp = ros::Time::now();
  
      diag_pub.publish(reading);

      loop_rate.sleep();
    }

    ctxClose(dev);
    
    return 0;
  }

void LoopbackControllerManager::ControllerManagerROSThread()
{
  ROS_INFO_STREAM("Callback thread id=" << boost::this_thread::get_id());

  ros::Rate rate(0.2/dt_);

  while (rosnode_->ok())
  {
    rate.sleep();
    ros::spinOnce();
  }
}

 bool spin()
 {
   ros::Rate loop_rate(this->framerate_);
   while (node_.ok())
   {
     if (!take_and_send_image())
       ROS_WARN("USB camera did not respond in time.");
     ros::spinOnce();
     loop_rate.sleep();
   }
   return true;
 }

void waitForAction(const T &action, const ros::NodeHandle &node_handle,
                   const ros::Duration &wait_for_server, const std::string &name)
{
  ROS_DEBUG("Waiting for MoveGroup action server (%s)...", name.c_str());

  // in case ROS time is published, wait for the time data to arrive
  ros::Time start_time = ros::Time::now();
  while (start_time == ros::Time::now())
  {
    ros::WallDuration(0.01).sleep();
    ros::spinOnce();
  }

  // wait for the server (and spin as needed)
  if (wait_for_server == ros::Duration(0, 0))
  {
    while (node_handle.ok() && !action->isServerConnected())
    {
      ros::WallDuration(0.02).sleep();
      ros::spinOnce();
    }
  }
  else
  {
    ros::Time final_time = ros::Time::now() + wait_for_server;
    while (node_handle.ok() && !action->isServerConnected() && final_time > ros::Time::now())
    {
      ros::WallDuration(0.02).sleep();
      ros::spinOnce();
    }
  }

  if (!action->isServerConnected())
    throw std::runtime_error("Unable to connect to move_group action server within allotted time (2)");
  else
    ROS_DEBUG("Connected to '%s'", name.c_str());
}

void MoveR::IK_loop()
{

  ros::Rate r(2);

  while(nh_.ok())
  {
    ros::spinOnce();
    IK4(Target);
    axis_pub.publish(arm_angle);
    r.sleep();
  }

  return;
}