rosbag相关

rosbag中有4个不同话题的数据，利用

std::vector<ctrl_msg::ctrl_ref> ctrl_msgs_vec = read_bag<ctrl_msg::ctrl_ref>(file_name, topic_name, -1);//读包中的某一个话题的所有数据
write_bag<ctrl_msg::ctrl_ref>(new_file_name, topic_name, cmds);//并将该话题的所有数据写入另一个包中

报错：

terminate called after throwing an instance of ‘rosbag::BagException’
what(): Tried to insert a message with time less than ros::TIME_MIN

利用rqt_plot画出了对应话题的曲线
正确情况的plot:
发现问题：
最后一个控制器指令没有时间戳，默认的时间戳是0，因此把最后一个控制指令的plot输出到了时间曲线的最前面

对应代码：

/*step1*/
void RVOSimulator::pub_dpos_for_ros(){
	ctrl_msg::ctrl_ref ctrl_ref_msg;
	ctrl_ref_msg.header.stamp = ros::Time::now();
	ctrl_ref_msg.ref_mask = 7;
	ctrl_ref_msg.yaw_ref = yawOffset;
	
	ctrl_ref_msg.pos_ref[0] = getAgentPosition(0).x();
	ctrl_ref_msg.pos_ref[1] = -getAgentPosition(0).y();
	ctrl_ref_msg.pos_ref[2] = -take_off_height;

	ctrl_ref_msg.vel_ref[0] = getAgentVelocity(0).x();
	ctrl_ref_msg.vel_ref[1] = -getAgentVelocity(0).y();
	ctrl_ref_msg.vel_ref[2] = 0.0f;

	ctrl_ref_msg.acc_ref[0] = 0.0f;
	ctrl_ref_msg.acc_ref[1] = 0.0f;
	ctrl_ref_msg.acc_ref[2] = 0.0f;

	ctrl_ref_pub.publish(ctrl_ref_msg);
}
/*step2*/
case HOVERING:
{	
	lg_node::agent_exec_state exec_state_;
	exec_state_.STATE = 5;
	exec_state_.id = uav_id;
	broadcast_agent_exec_state_pub.publish(exec_state_);
	if(first_exec_hovering){
		std::cout<<"uav_id: "<<uav_id<<" case HOVERING"<<std::endl;
		ctrl_msg::ctrl_ref hovering_command;
		hovering_command.header.stamp = ros::Time::now();//错误原因是没有写这行代码
		hovering_command.pos_ref[0] = agents_goals_[agents_goals_.size()-1].x();
		hovering_command.pos_ref[1] = -agents_goals_[agents_goals_.size()-1].y();
		hovering_command.pos_ref[2] = -take_off_height;
		hovering_command.yaw_ref = yawOffset;
		hovering_command.ref_mask = ctrl_msg::ctrl_ref::POS_CTRL_VALIED;
		for(int i=0; (i<10 && ros::ok()); i++) {
			ctrl_ref_pub.publish(hovering_command);
			ros::Duration(timeStep_).sleep();
  			}
		flag_exec_hovering = false;
		first_exec_hovering =false;
	}
	break;
}