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();
}
Ejemplo n.º 2
0
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;
}