Where can I find poses?

[uhvardhan]

Hi,

I have been using crazyswarm for the past week or so. I have been able to run most of the tutorials and example codes present.

I was wondering about the pose of each quadrotor. I have enabled pose_logging to True. Now I am wondering as to where I can find the relevant poses for each CF.

I just wanted to calculate how much the CF deviates from the ground truth.

P.S. I am using Loco Positioning System currently.

[jpreiss]

It gets broadcasted as a ros topic e.g. cf1/pose and through the ROS tf system:

crazyswarm/ros_ws/src/crazyswarm/src/crazyswarm_server.cpp

Lines 743 to 767 in 26edf4b

	void onPoseData(uint32_t time_in_ms, logPose* data) {
	if (m_enableLoggingPose) {
	geometry_msgs::PoseStamped msg;
	msg.header.stamp = ros::Time::now();
	msg.header.frame_id = "world";
	msg.pose.position.x = data->x;
	msg.pose.position.y = data->y;
	msg.pose.position.z = data->z;
	float q[4];
	quatdecompress(data->quatCompressed, q);
	msg.pose.orientation.x = q[0];
	msg.pose.orientation.y = q[1];
	msg.pose.orientation.z = q[2];
	msg.pose.orientation.w = q[3];
	m_pubPose.publish(msg);
	tf::Transform tftransform;
	tftransform.setOrigin(tf::Vector3(data->x, data->y, data->z));
	tftransform.setRotation(tf::Quaternion(q[0], q[1], q[2], q[3]));
	m_br.sendTransform(tf::StampedTransform(tftransform, ros::Time::now(), "world", frame()));
	}

This means it will be available through cf.position() in your script.

This should be documented.

@whoenig What happens when pose logging is enabled but a mocap system is also in use? This could occur when measuring the accuracy of a non-mocap positioning system. Per the linked code, won't the ROS tf jitter back and forth between the EKF estimate and the mocap?

[uhvardhan]

Thank you @jpreiss.

[whoenig]

I think having mocap and pose logging enabled would cause the jitter you mentioned and should be fixed. This should have no effect on correct operation, since tf is only used for visualization purposes, not for actual computations. Tracked in #572.