Timing problems when publishing an initial pose to amcl
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);
add a comment
