void OdometryHelperRos::getRobotVel(tf::Stamped<tf::Pose>& robot_vel) {
// Set current velocities from odometry
geometry_msgs::Twist global_vel;
{
boost::mutex::scoped_lock lock(odom_mutex_);
global_vel.linear.x = base_odom_.twist.twist.linear.x;
global_vel.linear.y = base_odom_.twist.twist.linear.y;
global_vel.angular.z = base_odom_.twist.twist.angular.z;
robot_vel.frame_id_ = base_odom_.child_frame_id;
}
robot_vel.setData(tf::Transform(tf::createQuaternionFromYaw(global_vel.angular.z), tf::Vector3(global_vel.linear.x, global_vel.linear.y, 0)));
robot_vel.stamp_ = ros::Time();
}
bool getGoalPose (const tf::TransformListener& tf,
const std::vector<geometry_msgs::PoseStamped>& global_plan,
const std::string& global_frame, tf::Stamped<tf::Pose>& goal_pose)
{
if (global_plan.empty())
{
ROS_ERROR ("Received plan with zero length");
return false;
}
const geometry_msgs::PoseStamped& plan_goal_pose = global_plan.back();
try
{
tf::StampedTransform transform;
tf.waitForTransform (global_frame, ros::Time::now(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, ros::Duration (0.5));
tf.lookupTransform (global_frame, ros::Time(),
plan_goal_pose.header.frame_id, plan_goal_pose.header.stamp,
plan_goal_pose.header.frame_id, transform);
poseStampedMsgToTF (plan_goal_pose, goal_pose);
goal_pose.setData (transform * goal_pose);
goal_pose.stamp_ = transform.stamp_;
goal_pose.frame_id_ = global_frame;
}
catch (tf::LookupException& ex)
{
ROS_ERROR ("No Transform available Error: %s\n", ex.what());
return false;
}
catch (tf::ConnectivityException& ex)
{
ROS_ERROR ("Connectivity Error: %s\n", ex.what());
return false;
}
catch (tf::ExtrapolationException& ex)
{
ROS_ERROR ("Extrapolation Error: %s\n", ex.what());
if (global_plan.size() > 0)
ROS_ERROR ("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int) global_plan.size(), global_plan[0].header.frame_id.c_str());
return false;
}
return true;
}