bool
AmclThread::get_odom_pose(tf::Stamped<tf::Pose>& odom_pose, double& x,
double& y, double& yaw,
const fawkes::Time* t, const std::string& f)
{
// Get the robot's pose
tf::Stamped<tf::Pose>
ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
tf::Vector3(0, 0, 0)), t, f);
try {
tf_listener->transform_pose(odom_frame_id_, ident, odom_pose);
} catch (Exception &e) {
if (cfg_buffer_debug_) {
logger->log_warn(name(), "Failed to compute odom pose (%s)",
e.what_no_backtrace());
}
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch, roll;
odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
//logger->log_info(name(), "Odom pose: (%f, %f, %f)", x, y, yaw);
return true;
}
bool
AmclNode::getOdomPose(tf::Stamped<tf::Pose>& odom_pose,
double& x, double& y, double& yaw,
const ros::Time& t, const std::string& f)
{
// Get the robot's pose
tf::Stamped<tf::Pose> ident (tf::Transform(tf::createIdentityQuaternion(),
tf::Vector3(0,0,0)), t, f);
try
{
this->tf_->transformPose(odom_frame_id_, ident, odom_pose);
}
catch(tf::TransformException e)
{
ROS_WARN("Failed to compute odom pose, skipping scan (%s)", e.what());
return false;
}
x = odom_pose.getOrigin().x();
y = odom_pose.getOrigin().y();
double pitch,roll;
odom_pose.getBasis().getEulerYPR(yaw, pitch, roll);
return true;
}