Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Hello,

We have done like below:

We used ar_pose_estimate to get pattern position from the robot and then subsribe to ar pose topic and get the robot pose.

msg_mark is the message received from ar_pose_marker

msg.header.frame_id = "/map";
msg.pose.pose.position.x = msg_mark->pose.pose.position.x;
msg.pose.pose.position.y = msg_mark->pose.pose.position.y;
msg.pose.pose.position.z = 0.0;
msg.pose.pose.orientation.w = msg_mark->pose.pose.orientation.w;

double theta;
btQuaternion quat;
quat.setRPY(0.0, 0.0, theta);

tf::quaternionTFToMsg(quat, pose.pose.pose.orientation);

msg.pose.covariance[6*0+0] = 0.5 * 0.5;

msg.pose.covariance[6*1+1] = 0.5 * 0.5;

msg.pose.covariance[6*3+3] = m_pi/12.0 * m_pi/12.0;

Regards,

Prasad