C++ Rate::sleep方法代码示例

    //================================================================
    // Close the gripper at the specified rate to the distance
    // specified
    //================================================================
    void closeToPosition(ros::Rate rate, double move_gripper_distance, 
                         double gripper_position)
    {
      // Get location of gripper currently 
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;

      // Continue until position is achieved, ros cancel, or if
      // the pressure approaches something dangerous
      while (current_gripper_position > gripper_position 
            && pressure_max < 500 
            && current_gripper_position > 0.0 
            && ros::ok())
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }

position_tracker::Position take_step(int action, ros::Rate loop_rate, ros::ServiceClient client)
{  
  printf("p take action\n");
  cout << "taking action " <<action << endl;
  /*Execute the Action */
  if(action==LEFT){
    left(client);
  }
  else if(action==FORWARD){
    forward(client);
  }
  else{
    right(client);
  }
  
  cout << "took an action" << endl;
  ros::spinOnce();
  loop_rate.sleep();
   
  //check to make sure this observation is ok
  
  position_tracker::Position observation= getObservation();
  return observation;
  /*
  cout << "going to convert"<< endl;
  convertObservation(observation);
  cout << "converted"<<endl;

  this_reward_observation.reward= calculateReward(observation);
  this_reward_observation.terminal=checkTerminal(observation);
  
    cout << "we took a step" << endl;
    return &this_reward_observation;
  */
}

    //================================================================
    // Close gripper to specified pressure
    //================================================================
    void closeToPressure(ros::Rate rate, int desired_pressure, 
                         double move_gripper_distance)
    {
      double current_gripper_position = simple_gripper->getGripperLastPosition();
      int pressure_max = 0;
      int pressure_min = 0;

      while (pressure_min < desired_pressure 
             && pressure_max < 500
             && ros::ok()
             && current_gripper_position > 0.0)
      {
        // Move the gripper by the specified amount
        simple_gripper->closeByAmount(move_gripper_distance);
        
        // Find and update if gripper moved
        current_gripper_position = simple_gripper->getGripperLastPosition();
      
        // Get pressure
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Wait set time and check again 
        ros::spinOnce();
        rate.sleep();
      }
    }

void ObjRecInterface::recognize_objects() 
{
  while(ros::ok() && !time_to_stop_) {
    // Don't hog the cpu
    ros::Duration(0.03).sleep();

    // Scope for syncrhonization
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_full(new pcl::PointCloud<pcl::PointXYZRGB>());
    {
      // Lock the buffer mutex
      boost::mutex::scoped_lock buffer_lock(buffer_mutex_);

      // Continue if the cloud is empty
      static ros::Rate warn_rate(1.0);
      if(clouds_.empty()) {
        ROS_WARN("Pointcloud buffer is empty!");
        warn_rate.sleep();
        continue;
      }

      ros::Time time_back, time_front;
      time_back.fromNSec(clouds_.back()->header.stamp * 1000);
      time_front.fromNSec(clouds_.back()->header.stamp * 1000);
      ROS_INFO_STREAM("Computing objects from "
          <<scene_points_->GetNumberOfPoints()<<" points "
          <<"between "<<(ros::Time::now() - time_back)
          <<" to "<<(ros::Time::now() - time_front)<<" seconds after they were acquired.");

      // Copy references to the stored clouds
      cloud_full->header = clouds_.front()->header;

      while(!clouds_.empty()) {
        *cloud_full += *(clouds_.front());
        clouds_.pop();
      }
    }

    objrec_msgs::RecognizedObjects objects_msg = do_recognition(cloud_full);

    // Publish the visualization markers
    this->publish_markers(objects_msg);

    // Publish the recognized objects
    objects_pub_.publish(objects_msg);

    // Publish the points used in the scan, for debugging
    /**
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr foreground_points_pcl(new pcl::PointCloud<pcl::PointXYZRGB>());
    foreground_points_pcl->header = cloud->header;
    for(unsigned int i=0; i<foreground_points->GetNumberOfPoints(); i++) {
      double point[3];
      foreground_points->GetPoint(i,point);
      foreground_points_pcl->push_back(pcl::PointXYZ(point[0]/1000.0,point[1]/1000.0,point[2]/1000.0));
    }
    foreground_points_pub_.publish(foreground_points_pcl);
    **/
  }
}

void turn(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count = 0;
	while(ros::ok()){
		count++;
    	out.publish(msg);
		rate.sleep();
		if (count == 40)break; //after 45 updates break
	}	
}	

void moveForward(ros::Publisher& out, geometry_msgs::Twist& msg, ros::Rate& rate){
	int count =0; //number of updates
    while(ros::ok()){
		out.publish(msg);		
    	rate.sleep();
		count++;
		if (count == 10) break; //after a second break
    }
}

void Node::mapPublishThread(ros::Rate rate)
{
  while(ros::ok()) {
    {
#ifdef USE_HECTOR_TIMING
      hector_diagnostics::TimingSection section("map publish");
#endif
      publishMap();
    }
    rate.sleep();
  }
}

    // TODO needs some love
    void run() {
        while(ros::ok()) {
            current_time = ros::Time::now();
            double dt = (current_time - last_time).toSec();
            // TODO these predicst and measure variables are not something that I understand
            if (predict) {
                prediction(dt);
                last_time = current_time;
                predict = false;

                if (measure) {
                    update_estimated_measurement();
                    weighting();
                    measure = false;
                }

                Normalization();
                resample();
            }

            if (visualization_enabled) {
                if (publish_particles) {
                    particles_pub.publish(loc_viz.get_particle_marker(particle_state));
                }
                if (publish_robot) {
                    robot_pub.publish(
                        loc_viz.get_robot_marker(std::vector<ras::sensor_info>(), x, y, theta, visualization_use_distance));
                }
                if (publish_thickened_walls) {
                    wall_cell_pub.publish(loc_viz.get_thick_wall_grid_cells());
                }
                if (publish_walls) {
                    grid_cell_pub.publish(loc_viz.get_wall_grid_cells());
                }
                if (publish_path) {
                    point_path_pub.publish(loc_viz.add_to_path(x, y));
                }
            }

            //Publish Geometry msg of Predicted postion
            geometry_msgs::Pose2D msg;
            msg.x = x;
            msg.y = y;
            msg.theta = theta;
            position_pub.publish(msg);

            //Use if add a subscription(Add as good measure)
            ros::spinOnce();
            //Delays untill it is time to send another message
            rate->sleep();
            last_time = current_time;
        }
    }

position_tracker::Position start_environment(ros::Rate loop_rate)
{
  /* see where the robot is.  And then send that position*/
  
  ros::spinOnce();
  loop_rate.sleep();
  
  position_tracker::Position observation= getObservation();
  
  cout << observation.x << " " << observation.y << " " << observation.theta << endl;  
  return observation;
  
}

    //================================================================ 
    // Closes the gripper until a pressure is achieved, 
    // then opens again at the same rate the gripper closed at
    //================================================================
    void squeeze(ros::Rate rate, double move_gripper_distance)
    {
      int previous_pressure_max = 0; 
      int pressure_max = 0;
      int no_motion_counter = 0;

      // Close 
      while (pressure_max < SqueezePressureContact && ros::ok()
             && no_motion_counter < 250 
	           && simple_gripper->getGripperLastPosition() > 0.0)
      {
        previous_pressure_max = pressure_max;
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        // Checks if pressure has been "stuck" 
        if (abs(previous_pressure_max-pressure_max) < 1)
          no_motion_counter++;

        simple_gripper->closeByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
   
      // Store the gripper position when max pressure is achieved
      gripper_max_squeeze_position = simple_gripper->getGripperLastPosition();

      // Open - 10 and not 0 because the values will drift
      while (pressure_max > 10 && ros::ok() 
	     && simple_gripper->getGripperLastPosition() < 0.08)
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->openByAmount(move_gripper_distance);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }

    //================================================================
    // Open gripper by the rate and position specified.
    // This is necessary to keep opening the gripper until 
    // the bioTacs do not report any pressure 
    //================================================================
    void openUntilNoContact(ros::Rate rate, double gripper_position)
    {
      int pressure_max = LightPressureContact + 50;

      while (pressure_max > LightPressureContact && ros::ok())
      {
        pressure_max = max(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);

        simple_gripper->open2Position(gripper_position);
        //ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }

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


	ros::init(argc, argv,"button_led");

	ROS_INFO("Started Odroid-C1 Button Blink Node");

	wiringPiSetup ();
	pinMode(LED, OUTPUT);


	ros::NodeHandle n;

	ros::Subscriber sub = n.subscribe("led_blink",10,blink_callback);
  	ros::Publisher chatter_pub = n.advertise<std_msgs::Bool>("led_blink", 10);


	std_msgs::Bool on = true;
	std_msgs::Bool off = false;


 	while (ros::ok())
  	{

	        if (digitalRead(butPin)) // Button is released if this returns 1
		{
	
			chatter_pub.publish(on);

		}
		else
		{

			chatter_pub.publish(off);

		}

		ros::spinOnce();

    		loop_rate.sleep();

	}

	



}

 void run(){
     
     RNG rng(12345);
     while(ros::ok()) {
         current_time = ros::Time::now();
         //computer odemetry using integration
         double dt = (current_time - last_time).toSec();
         //Prediction model
         Prediction(dt);
         //Measurement model
         EstimateMeasurement_Update();
         RealMeasurement_Update();
         Innovation();
         ReSampling();
         Normalization();
         
         // Show all the particles on the map
         image = imread(PATH,CV_LOAD_IMAGE_COLOR);
         
         Scalar color = Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255));
         for(int i=0;i<Particles;++i){
             particle_position = Point(particle_state[1][i]*100+5,particle_state[0][i]*100+5);
             circle(image,particle_position,4,color,-1,8,0);
         }
         
         // Show the final predicted position on the map
         circle(postion_image,position,4,color,-1,8,0);
         
         //Show odometry localization on the map
         namedWindow( OPENCV_WINDOW, CV_WINDOW_NORMAL );// Create a window for display.
         imshow( OPENCV_WINDOW, image );                  // Show our map inside it.
         namedWindow( OPENCV_WINDOW1, CV_WINDOW_NORMAL );// Create a window for display.
         imshow( OPENCV_WINDOW1, postion_image);
         
         //Publish Geometry msg of Predicted postion
         msg.x = x;
         msg.y = y;
         msg.theta = theta;
         position_pub.publish(msg);
         
         waitKey(3);                                 // Wait for a keystroke in the window
         last_time = current_time;
         //Use if add a subscription(Add as good measure)
         ros::spinOnce();
         //Delays untill it is time to send another message
         rate->sleep();
     }
 }

//Moves forward for 3 seconds, moves backwards 3 seconds
//Press Enter for emergency land
void controller::testMove(ros::Rate loop_rate, ros::NodeHandle node)
{
	if (inFlight == 0)
	{
		controller::sendTakeoff(node);
		inFlight = 1;
	}

	start_time = (double)ros::Time::now().toSec();
	cout << "Started Time: " << start_time << "\n" << endl;

	linebufferedController(false);
	echoController(false);

	while ((double)ros::Time::now().toSec() < start_time + test_flight_time)
	{

		if ((double)ros::Time::now().toSec() < start_time + test_flight_time / 2)
		{
			controller::setMovement(5, 0, 0);
			controller::sendMovement(node);

		}
		else
		{
			controller::setMovement(-5,0, 0);
			controller::sendMovement(node);
		}

		ros::spinOnce();
		loop_rate.sleep();
		if (iskeypressed(500) && cin.get() == '\n') break;
	}

	controller::resetTwist();
	controller::sendMovement(node);
	controller::sendLand(node);

	echoController();
	linebufferedController();
}

void
computeBackgroundCloud (int frames, float voxel_size, int sr_conf_threshold, std::string frame_id, ros::Rate rate, PointCloudT::Ptr& background_cloud)
{
  std::cout << "Background acquisition..." << std::flush;

  // Create background cloud:
  PointCloudT::Ptr cloud_to_process(new PointCloudT);
  background_cloud->header = cloud->header;
  for (unsigned int i = 0; i < frames; i++)
  {
    *cloud_to_process = *cloud;

    // Remove low confidence points:
    removeLowConfidencePoints(confidence_image, sr_conf_threshold, cloud_to_process);

    // Voxel grid filtering:
    PointCloudT::Ptr cloud_filtered(new PointCloudT);
    pcl::VoxelGrid<PointT> voxel_grid_filter_object;
    voxel_grid_filter_object.setInputCloud(cloud_to_process);
    voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
    voxel_grid_filter_object.filter (*cloud_filtered);

    *background_cloud += *cloud_filtered;
    ros::spinOnce();
    rate.sleep();
  }

  // Voxel grid filtering:
  PointCloudT::Ptr cloud_filtered(new PointCloudT);
  pcl::VoxelGrid<PointT> voxel_grid_filter_object;
  voxel_grid_filter_object.setInputCloud(background_cloud);
  voxel_grid_filter_object.setLeafSize (voxel_size, voxel_size, voxel_size);
  voxel_grid_filter_object.filter (*cloud_filtered);

  background_cloud = cloud_filtered;

  // Background saving:
  pcl::io::savePCDFileASCII ("/tmp/background_" + frame_id.substr(1, frame_id.length()-1) + ".pcd", *background_cloud);

  std::cout << "done." << std::endl << std::endl;
}