C++ ros::Rate类代码示例

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);
    **/
  }
}

    //================================================================
    // 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 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();
  }
}

    //================================================================ 
    // 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();
      }
    }

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

    //================================================================
    // 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();
      }
    }

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

hmc5883l::hmc5883l(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate, hmc5883l_callBackFunc dataReadyCallBack = 0) {
	ROS_INFO("HMC5883L : Initializing");
	char str[80];
	i2c = i2c_ptr;
	sem_init(&lock, 0, 1);
	ctl_reg_a = (0x03 << HMC5883L_MA0) | (0x06 << HMC5883L_DO0) | (0x00 << HMC5883L_MS0);
	i2c->write_byte(addr, HMC5883L_CONF_REG_A, ctl_reg_a);

	mode_reg = (0x00 << HMC5883L_MD0);
	i2c->write_byte(addr, HMC5883L_MODE_REG, mode_reg);

	setScale(hmc5883l_scale_130);

	nh = nh_ptr;
	timer = nh->createTimer(rate, &hmc5883l::timerCallback, this);
	if (dataReadyCallBack)
		dataCallback = dataReadyCallBack;

	sprintf(str, "Sampling @ %2.2f Hz", (float) 1.0f / rate.expectedCycleTime().toSec());
	ROS_INFO("HMC5883L : \t%s", str);
	ROS_INFO("HMC5883L : Initialization done");
}

micromag::micromag(i2cfile* i2c_ptr, ros::NodeHandle* nh_ptr, ros::Rate rate,
                   micromag_callBackFunc dataReadyCallBack = 0, int windowSize = 32) {
	ROS_INFO("micromag : Initializing");
	char str[80];
	i2c = i2c_ptr;

	sem_init(&lock, 0, 1);

	double freq = 1 / rate.expectedCycleTime().toSec();

	setWindow(windowSize);

	nh = nh_ptr;
	timer = nh->createTimer(rate, &micromag::timerCallback, this);
	if (dataReadyCallBack)
		dataCallback = dataReadyCallBack;

	sprintf(str, "Sampling @ %2.2f Hz", freq);
	ROS_INFO("micromag : \t%s", str);
	ROS_INFO("micromag : Initialization done");

	// Probe device... Return 0 on failure..
}

    //================================================================
    // Higher level motion to close gripper until contact is found
    // Pass in the rate at which contact is closed at and the 
    // distance the gripper moves rate is in Hz, 
    // move_gripper_distance in meters
    // Velocity gripper moves is found by how much moved by what rate
    //================================================================
    void findContact(ros::Rate rate, double move_gripper_distance)
    {
      int pressure_min = 0; 
      int pressure_max = 0;
      bool contact_found = false;
      int no_motion_counter = 0;
      int previous_pressure_max = 0;

      // Close until minimum pressure is found - however stop if
      // any finger has too much pressure
      while (pressure_min < LightPressureContact && ros::ok()
             && pressure_max < 600  && no_motion_counter < 250)
      {
        // Check pressure min and max
        simple_gripper->closeByAmount(move_gripper_distance);
        pressure_min = min(biotac_obs->pressure_normalized_[Left], biotac_obs->pressure_normalized_[Right]);
        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++;

        // Set distance for object width 
        if (!contact_found && pressure_min > 10){
          gripper_initial_contact_position = simple_gripper->getGripperLastPosition();
          contact_found = true;
        }

        previous_pressure_max = pressure_max;

        //ROS_INFO("Pressure Min is: [%d]", pressure_min);
       // ROS_INFO("Pressure Max is: [%d]", pressure_max);
        ros::spinOnce();
        rate.sleep();
      }
    }