Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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[60+0] = 0.5 * 0.5; initialPose.pose.covariance[61+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[60+0] = 0.5 * 0.5; pose.pose.covariance[61+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);

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[60+0] = 0.5 * 0.5; initialPose.pose.covariance[61+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[60+0] = 0.5 * 0.5; pose.pose.covariance[61+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);