ROS 时间控制

Stupid && Lazy BUG

今天想偷懒实现一个小功能的时候遇到了一个小bug, 在已知车辆位姿的情况下，手动发布twist话题使得车辆旋转一定的角度，使得车辆的位姿与目标位姿一致，当时我主要想到两种写法：
①：计算角度差，旋转，然后不断更新角度差，直到位姿一致。
②：仅仅计算一次角度差，然后根据旋转速度推测旋转时间。

正常情况下这不是个值得思考的问题，我一般直接用第一种，因为写起来简单顺手，但是今天突然想直接用第二种方式，起初我是这么写的：

void funA()
{
	ros::Rate loop_time(10);
	double headingError = current_pose.orientation - goal_pose.orientation;
	double estimatedTimeCost = fabs(headingError/Twist.angular.z);
	clock_t start_count_timer = clock();
	while((double)(clock() - start_count_timer)/CLOCKS_PER_SEC < estimatedTimeCost)
	{
		if(headingError > 0 && headingError <= M_PI)
		{
			Twist.angular.z = -0.1;
		}else
		{
			Twist.angular.z = 0.1;
		}
		twistPub.publish(Twist);
		loop_rate.sleep();
	}
}

也就是说，假如车辆当前朝向与目标朝向存在180误差，弧度为3.14，若车辆的旋转角速度为0.1弧度每秒，调整位姿需要花费（3.14 / 0.1 = 31.4s ）,也就是这里会延时31.4秒，但是实际上上面的实现方式所花费的时间是超出31.4s的！

根据ros::Rate 的性质， while循环被控制在了10hz, 也就是说每一次while循环 clock() - start_count_timer是以0.1s的速度更新，整个while都在以10hz速度更新， 那么意味着在计时过程中时间不是连续增加的，而是每增加0.1s就休眠，虽然时间是正确的，但是程序变慢了。

后面我将ros::Rate 调成50hz,效果依然不好，后面我将 ros的loop_rate.sleep()去掉，发现时间几乎正确了但是这是Twist的发布频率是非常高的，属于对系统性能的无效占用，这无疑又会埋下一个BUG。

ROS::Timer && ROS::Duration

ros Timer的使用，通过ros::Timer 可以实现相关函数的定时/周期性调用，WallTimer 同理。

ros::Timer timer = nh.createTimer(ros::Duration(0.1), callback);
void callback(const ros::TimerEvent& event)
{
}

ros::Duration(0.5).sleep(); 可以实现延时0.5s. 上面用while + loop_rate 对于时间的精度控制不如ros::Timer.