Ask Your Question
0

Timing problems when publishing an initial pose to amcl

asked 2013-08-04 22:50:20 -0500

madmax gravatar image

updated 2013-11-18 07:26:34 -0500

tfoote gravatar image

I am trying to publish a pose to the initial pose subscriber of amcl.
But I always get a transformation error although I tried different setups with adding/subtracting time.

Failed to transform initial pose in time (Unable to lookup transform, cache is empty, when looking up transform from frame [/robot_5/base_link] to frame [/robot_5/map])

Time difference[s]: 0.001029

geometry_msgs::PoseWithCovarianceStamped initialPose;
initialPose.header.frame_id = global_frame_;
initialPose.header.stamp = ros::Time::now();// - ros::Duration(0.1);//triangle.laserm_A.stamp + ros::Duration(0.03);
initialPose.pose.pose.position.x = result.getX();
initialPose.pose.pose.position.y = result.getY();
initialPose.pose.pose.position.z = 0;

tf::Quaternion q = tf::createQuaternionFromYaw(theta);
geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(q, qMsg);
initialPose.pose.pose.orientation = qMsg;

initialPose.pose.covariance[6*0+0] = 0.5 * 0.5;
initialPose.pose.covariance[6*1+1] = 0.5 * 0.5;
initialPose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", result.getX(), result.getY(), theta, global_frame_.c_str());

initial_pose_pub_.publish(initialPose);

Then I tried the setup from rviz, with no setting a stamp at all.
Doesn't work, and I don't understand how that can work in rviz??

geometry_msgs::PoseWithCovarianceStamped pose;
pose.header.frame_id = fixed_frame;
pose.pose.pose.position.x = x;
pose.pose.pose.position.y = y;

tf::Quaternion quat;
quat.setRPY(0.0, 0.0, theta);
tf::quaternionTFToMsg(quat,
                    pose.pose.pose.orientation);
pose.pose.covariance[6*0+0] = 0.5 * 0.5;
pose.pose.covariance[6*1+1] = 0.5 * 0.5;
pose.pose.covariance[6*5+5] = M_PI/12.0 * M_PI/12.0;
ROS_INFO("Setting pose: %.3f %.3f %.3f [frame=%s]", x, y, theta, fixed_frame.c_str());
pub_.publish(pose);

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2013-08-05 04:16:16 -0500

madmax gravatar image

Found out that this warning message also comes when you call initialpose from rviz.
So this warning gets always called when you set initial pose...

nasty...

edit flag offensive delete link more

Comments

hi @madmax, is there a way to fix this??

Aarif gravatar imageAarif ( 2014-12-07 11:34:08 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2013-08-04 22:50:20 -0500

Seen: 1,258 times

Last updated: Aug 05 '13