Function getTelemetry

clover/src/simple_offboard.cpp:235–318

#define TIMEOUT(msg, timeout) (msg.header.stamp.isZero() || (ros::Time::now() - msg.header.stamp > timeout))

bool getTelemetry(GetTelemetry::Request& req, GetTelemetry::Response& res)
{
	ros::Time stamp = ros::Time::now();

	if (req.frame_id.empty())
		req.frame_id = local_frame;

	res.frame_id = req.frame_id;
	res.x = NAN;
	res.y = NAN;
	res.z = NAN;
	res.lat = NAN;
	res.lon = NAN;
	res.alt = NAN;
	res.vx = NAN;
	res.vy = NAN;
	res.vz = NAN;
	res.roll = NAN;
	res.pitch = NAN;
	res.yaw = NAN;
	res.roll_rate = NAN;
	res.pitch_rate = NAN;
	res.yaw_rate = NAN;
	res.voltage = NAN;
	res.cell_voltage = NAN;

	if (!TIMEOUT(state, state_timeout)) {
		res.connected = state.connected;
		res.armed = state.armed;
		res.mode = state.mode;
	}

	try {
		waitTransform(req.frame_id, fcu_frame, stamp, telemetry_transform_timeout);
		auto transform = tf_buffer.lookupTransform(req.frame_id, fcu_frame, stamp);
		res.x = transform.transform.translation.x;
		res.y = transform.transform.translation.y;
		res.z = transform.transform.translation.z;

		double yaw, pitch, roll;
		tf2::getEulerYPR(transform.transform.rotation, yaw, pitch, roll);
		res.yaw = yaw;
		res.pitch = pitch;
		res.roll = roll;
	} catch (const tf2::TransformException& e) {
		ROS_DEBUG("%s", e.what());
	}

	if (!TIMEOUT(velocity, velocity_timeout)) {
		try {
			// transform velocity
			waitTransform(req.frame_id, fcu_frame, velocity.header.stamp, telemetry_transform_timeout);
			Vector3Stamped vec, vec_out;
			vec.header.stamp = velocity.header.stamp;
			vec.header.frame_id = velocity.header.frame_id;
			vec.vector = velocity.twist.linear;
			tf_buffer.transform(vec, vec_out, req.frame_id);