C++ rate函数代码示例

void MediaPlayerPrivateAVFoundation::timeChanged(double time)
{
    LOG(Media, "MediaPlayerPrivateAVFoundation::timeChanged(%p) - time = %f", this, time);

    if (m_seekTo == invalidTime)
        return;

    // AVFoundation may call our observer more than once during a seek, and we can't currently tell
    // if we will be able to seek to an exact time, so assume that we are done seeking if we are
    // "close enough" to the seek time.
    const double smallSeekDelta = 1.0 / 100;

    float currentRate = rate();
    if ((currentRate > 0 && time >= m_seekTo) || (currentRate < 0 && time <= m_seekTo) || (abs(m_seekTo - time) <= smallSeekDelta)) {
        m_seekTo = invalidTime;
        updateStates();
        m_player->timeChanged();
    }
}

int main(int argc, char** argv){
  ros::init(argc, argv, "pose_to_tf");
  ros::NodeHandle node("~");
  node.param("frame_id", frame_id, std::string("frame_id_not_specified"));
  tf::TransformBroadcaster br;
  started = false;

  ros::NodeHandle node_top;
  ros::Subscriber sub = node_top.subscribe("pose", 10, &poseCallback);

  ros::Rate rate(50.0);
  while (ros::ok()){
    if (started)
      br.sendTransform(transform);
    ros::spinOnce();
    rate.sleep();
  }
  return 0;
}

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");
  ros::NodeHandle node;
  
  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  while (node.ok()){
    transform.setOrigin( tf::Vector3(2.0*sin(ros::Time::now().toSec()),
				     2.0*cos(ros::Time::now().toSec()),
				     0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(),
					  "turtle1", "carrot1"));
    rate.sleep();
  }
  return 0;
};

int main(int argc, char** argv){
  ros::init(argc, argv, "laser");
  ros::NodeHandle node;
  ros::Publisher vis_pub = node.advertise<visualization_msgs::Marker>( "imu", 0 );
  visualization_msgs::Marker marker;
  marker.header.frame_id = "map";
  marker.header.stamp = ros::Time();
  marker.ns = "imu";
  marker.id = 0;
  marker.type = visualization_msgs::Marker::CUBE;
  marker.action = visualization_msgs::Marker::ADD;
  marker.pose.position.x = 0;
  marker.pose.position.y = 0;
  marker.pose.position.z = 0;
  marker.pose.orientation.x = 0.0;
  marker.pose.orientation.y = 0.0;
  marker.pose.orientation.z = 0.0;
  marker.pose.orientation.w = 1.0;
  marker.scale.x = 1;
  marker.scale.y = 0.1;
  marker.scale.z = 0.1;
  marker.color.a = 1.0; // Don't forget to set the alpha!
  marker.color.r = 0.0;
  marker.color.g = 1.0;
  marker.color.b = 0.0;

  tf::TransformBroadcaster br;
  tf::Transform transform;

  ros::Rate rate(10.0);
  
  tf::Vector3 origin(0.,0.,0.);
  while (node.ok()){
    transform.setOrigin(origin);
    transform.setRotation( tf::Quaternion(0, 0, 0, 1) );
    std::cout << "[" << origin.x() << ", " << origin.y() << ", " << origin.z() << "]" << std::endl;
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "map", "laser"));
    vis_pub.publish( marker );
    rate.sleep();
    //origin = addToOrigin(origin, tf::Vector3(0.1,0,0));
  }
  return 0;
}

//================================================================
// Get current transform of the arm - from torso_lift_link
// to the gripper tool frame using tf_listener
//================================================================
void biotacArmController::getArmTransform()
{
  bool noTransform = true;
  ros::Rate rate(1);

  while (noTransform && ros::ok())
  {
    try 
    {
      tf_listener->lookupTransform(ReferenceLink, "/l_gripper_tool_frame", ros::Time(0), *store_transform);
      noTransform = false;
    }
    catch (tf::TransformException ex)
    {
      ROS_INFO("No valid transform. Trying again");
    }
    rate.sleep();
  }  
}

bool WorldDownloadManager::shiftNear(const Eigen::Affine3f & pose, float distance)
{
  ROS_INFO("kinfu: shiftNear...");
  m_kinfu->shiftNear(pose,distance);
  if (!m_kinfu->isShiftComplete()) // shifting is waiting for cube
  {
    ros::Rate rate(10);
    do
    {
      rate.sleep();
      m_kinfu->shiftNear(pose,distance);
    }
    while (!m_kinfu->isShiftComplete() || ros::isShuttingDown());
  }

  if (ros::isShuttingDown())
    return false;
  return true;
}

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

    //Set up the node and nodehandle.
    ros::init(argc, argv, "tf_listener");
    ros::NodeHandle nh;
    ROS_INFO_STREAM("Started node tf_listener.");
  
    std::string target_frame;
    std::string source_frame;
    tf::TransformListener tf_listener;
    tf::StampedTransform transform;
    ros::Rate rate(1.); //used to throttle execution
    
    if (argc < 3) {//if no arguments are given, use base_link and part as the default frame names
        target_frame = "/base_link";
        source_frame = "/part";
    }
    else {//if there are at least 2 arguments given, use them as frame names
        target_frame = argv[1];
        source_frame = argv[2];
    }    
        
    while (ros::ok()) {
        try {
            //find transform from target TO source (naming is a bit confusing),
            //at the current time (ros::Time()), and assign result to transform.
            tf_listener.lookupTransform(target_frame, source_frame, ros::Time(), transform);
            
            //copy transform to a msg type to utilize stringstream properties
            //show transform, and distance between two transforms
            geometry_msgs::Transform buffer;
            tf::transformTFToMsg(transform, buffer);
            ROS_INFO_STREAM("Transform from " << target_frame << " to " << source_frame << ": " << std::endl << buffer);
            ROS_INFO_STREAM("Distance between transforms: " << transform.getOrigin().length() << " meters.");
        }
        catch (...) {//assume that the exception thrown is because transform is not available yet
            ROS_WARN_STREAM("Waiting for transform from " << target_frame << " to " << source_frame);
        }
        rate.sleep();   //throttle execution (1 second default)
    }
    return 0;
}

// Drive a specified distance (based on base odometry information) with given velocity Twist (vector)
bool Base::drive(double distance, const geometry_msgs::Twist& velocity)
{
	tf::StampedTransform start_transform;
	tf::StampedTransform current_transform;

	// Wait and get transform
	tf_listener_odom_.waitForTransform("base_footprint", "odom_combined", ros::Time(0), ros::Duration(5.0));
	tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), start_transform);

	// Set cmd_ to velocity and clear angular and linear.z;
	cmd_ = velocity;
	cmd_.angular.x = cmd_.angular.y = cmd_.angular.z = cmd_.linear.z = 0;



	// Loop until pos reached
	ros::Rate rate(PUBLISH_RATE_);
	bool done = false;

	while (!done && nh_.ok()) {
		// Send the drive command
		pub_base_vel_.publish(cmd_);
		rate.sleep();

		// Get the current transform
		try {
			tf_listener_odom_.lookupTransform("base_footprint", "odom_combined", ros::Time(0), current_transform);
		} catch (tf::TransformException ex) {
			ROS_ERROR("%s",ex.what());
			break;
		}

		// See how far we've traveled
		tf::Transform relative_transform = start_transform.inverse() * current_transform;
		double dist_moved = relative_transform.getOrigin().length();


		if (dist_moved >= distance)
			done = true;
	}
	return done;
}

int main(int argc,char **argv) {
	init();
	ros::init(argc,argv, "Controller");
	ros::NodeHandle nh;
	ros::Subscriber sub_state = nh.subscribe("auv/state", 1000, &stateListenerCallBack);
	ros::Subscriber sub_cmd_vel = nh.subscribe("sim/cmd_vel", 1000, &cmd_velCallBack);
	ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("zeabus/cmd_vel",1000);
	ros::Rate rate(100);
	while(ros::ok) {
		setPreviousState();
		ros::spinOnce();
		pub.publish(calculatePID());
		//printf("Current velo : %lf %lf %lf (%lf %lf %lf)\n",vel[0],vel[1],vel[2],cmd_vel[0],cmd_vel[1],cmd_vel[2]);
		printf(" x = %lf \n",position[3]);
		printf("x=%lf y=%lf z=%lf\n",position[0],position[1],position[2]);	
		printf("roll=%lf pitch=%lf yaw=%lf\n",position[3],position[4],position[5]);
		rate.sleep();
	}
	ros::shutdown();
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "my_tf_listener");
    ROS_INFO("hello ros_info");
    ros::NodeHandle node;

    ros::service::waitForService("spawn");
    ros::ServiceClient add_turtle =
        node.serviceClient<turtlesim::Spawn>("spawn");
    turtlesim::Spawn srv;
    add_turtle.call(srv);

    ros::Publisher turtle_vel =
        node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 2);

    tf::TransformListener listener;

    ros::Rate rate(10.0);
    while (node.ok()) {
        tf::StampedTransform transform;
        try {
            listener.lookupTransform("/turtle2", "/carrot1",
                                     ros::Time(0), transform);
        }
        catch (tf::TransformException &ex) {
            ROS_ERROR("%s",ex.what());
            ros::Duration(1.0).sleep();
            continue;
        }

        geometry_msgs::Twist vel_msg;
        vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
                                        transform.getOrigin().x());
        vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
                                      pow(transform.getOrigin().y(), 2));
        turtle_vel.publish(vel_msg);
        std::cout<<transform.getOrigin().x()<<" "<<transform.getOrigin().y()<<std::endl;
        //ROS_INFO("%d %d",transform.getOrigin().x(),transform.getOrigin().y());
        rate.sleep();
    }
    return 0;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "displacement");

  ros::NodeHandle node;
  ros::Subscriber sub1 = node.subscribe( "turtle1/pose", 10, &poseCallback1 );
  ros::Subscriber sub2 = node.subscribe( "turtle2/pose", 10, &poseCallback2 );
  ros::Publisher pub = node.advertise<geometry_msgs::Transform>( "displacement/transform", 1000 );

  //Set the loop at 10 HZ
  ros::Rate rate(10);

  while (ros::ok())
  {
      ros::spinOnce();//Call this function to process ROS incoming messages.

      //Calculate the transformation
      tf::Transform transformTmp;
      // transformTmp = transform1.inverse() * transform2;

      tf::Vector3 t1Origin = transform1.getOrigin();
      tf::Vector3 t2Origin = transform2.getOrigin();

      tf::Quaternion q1 = transform1.getRotation();
      tf::Quaternion q2 = transform2.getRotation();

      transformTmp.setOrigin( tf::Vector3(t2Origin.getX() - t1Origin.getX(), t2Origin.getY() - t1Origin.getY(), 0.0) );
      tf::Quaternion qtemp;
      qtemp.setRPY(0, 0, q2.getAngle() - q1.getAngle());
      transformTmp.setRotation(qtemp);

      //Publish the transformation
      geometry_msgs::Transform tg;
      tf::transformTFToMsg(transformTmp, tg);
      pub.publish(tg);

      rate.sleep();//Sleep the rest of the cycle until 10 Hz
    }

    return 0;
  }

void GraspLocalizer::localizeGrasps()
{
	ros::Rate rate(1);
	std::vector<int> indices(0);

	while (ros::ok())
	{
    // wait for point clouds to arrive
		if (num_clouds_received_ == num_clouds_)
		{
      // localize grasps
			if (num_clouds_ > 1)
			{
				PointCloud::Ptr cloud(new PointCloud());
				*cloud = *cloud_left_ + *cloud_right_;
				hands_ = localization_->localizeHands(cloud, cloud_left_->size(), indices, false, false);
			}
			else
			{
				hands_ = localization_->localizeHands(cloud_left_, cloud_left_->size(), indices, false, false);
			}

			antipodal_hands_ = localization_->predictAntipodalHands(hands_, svm_file_name_);
			handles_ = localization_->findHandles(antipodal_hands_, min_inliers_, 0.005);

      // publish handles
			grasps_pub_.publish(createGraspsMsg(handles_));
			ros::Duration(1.0).sleep();

      // publish hands contained in handles
			grasps_pub_.publish(createGraspsMsgFromHands(handles_));
			ros::Duration(1.0).sleep();

      // reset
			num_clouds_received_ = 0;
		}

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

void TfSubscriber::run()
{
   ros::Rate rate(10.0);
   while (node.ok()){
     tf::StampedTransform transform;
     try{
       listener.lookupTransform("map", "base_footprint", ros::Time(0), transform);
     }
     catch (tf::TransformException &ex) {
       ROS_ERROR("%s",ex.what());
       ros::Duration(1.0).sleep();
       continue;
     }
     ROS_INFO("---------------------TF VALUES-----------------------");
     ROS_INFO("\tX Value: %.8f", transform.getOrigin().x());
     ROS_INFO("\tY Value: %.8f", transform.getOrigin().y());
     ROS_INFO("\tZ Value: %.8f", transform.getOrigin().z());
     ROS_INFO("-----------------------------------------------------");
     rate.sleep();
  }
}

int main(int argc, char **argv){
	ros::init(argc,argv,"pubvel_toggle");
	ros::NodeHandle nh;

	ros::ServiceServer server = 
		nh.advertiseService("toggle_forward",&toggleForward);

	ros::Publisher pub=nh.advertise<geometry_msgs::Twist>(
		"turtle1/cmd_vel",1000);
	ros::Rate rate(2);
	while(ros::ok()){
		geometry_msgs::Twist msg;
		msg.linear.x = forward?1.0:0.0;
		msg.angular.z=forward?0.0:1.0;
		pub.publish(msg);
		ros::spinOnce();
		rate.sleep();
	}


}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "detector_filter");

  ros::NodeHandle node;
  ros::Publisher pose_pub = node.advertise<geometry_msgs::PoseWithCovarianceStamped>("pose_out", 10);
  geometry_msgs::PoseWithCovarianceStamped pose;
  detector::DetectorFilter detector_filter;

  ros::Rate rate(100.0);
  while (ros::ok()){
    if (detector_filter.getPose(pose)){
      pose.header.stamp = ros::Time::now();
      pose_pub.publish(pose);
    }
    ros::spinOnce();
    rate.sleep();
  }

  return 0;
}