robot_utils.cpp

// Copyright (c) 2018 Intel Corporation
// Copyright (c) 2019 Steven Macenski
// Copyright (c) 2019 Samsung Research America
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include <string>
#include <cmath>
#include <memory>
 
#include "tf2/convert.hpp"
#include "tf2/utils.hpp"
#include "nav2_ros_common/tf2_factories.hpp"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
 
#include "nav2_util/robot_utils.hpp"
#include "rclcpp/logger.hpp"
 
namespace nav2_util
{
 
bool lookupTransformWithStalenessCheck(
  nav2::TransformBuffer & tf_buffer,
  const std::string & target_frame,
  const std::string & source_frame,
  const rclcpp::Time & current_time,
  double staleness_threshold,
  geometry_msgs::msg::TransformStamped & transform)
{
  if (target_frame == source_frame) {
    geometry_msgs::msg::TransformStamped identity;
    identity.header.frame_id = target_frame;
    identity.header.stamp = current_time;
    identity.child_frame_id = source_frame;
    identity.transform.rotation.w = 1.0;
    transform = identity;
    return true;
  }
 
  geometry_msgs::msg::TransformStamped latest_transform;
  try {
    latest_transform = tf_buffer.lookupTransform(
      target_frame, source_frame, tf2::TimePointZero);
  } catch (const tf2::TransformException & ex) {
    RCLCPP_ERROR(
      rclcpp::get_logger("lookupTransformWithStalenessCheck"),
      "Failed to get transform: %s", ex.what());
    return false;
  }
 
  const bool has_timestamp =
    latest_transform.header.stamp.sec != 0 || latest_transform.header.stamp.nanosec != 0;
  if (staleness_threshold > 0.0 && has_timestamp) {
    const auto transform_time = rclcpp::Time(
      latest_transform.header.stamp, current_time.get_clock_type());
    const double transform_age = (current_time - transform_time).seconds();
    if (transform_age > staleness_threshold) {
      RCLCPP_ERROR(
        rclcpp::get_logger("lookupTransformWithStalenessCheck"),
        "Transform from frame '%s' to frame '%s' is stale: age %fs exceeds threshold %fs",
        source_frame.c_str(), target_frame.c_str(), transform_age, staleness_threshold);
      return false;
    }
  }
  transform = latest_transform;
  return true;
}
 
geometry_msgs::msg::PoseStamped transformToPoseStamped(
  const geometry_msgs::msg::TransformStamped & transform)
{
  geometry_msgs::msg::PoseStamped pose;
  pose.header = transform.header;
  pose.pose.position.x = transform.transform.translation.x;
  pose.pose.position.y = transform.transform.translation.y;
  pose.pose.position.z = transform.transform.translation.z;
  pose.pose.orientation = transform.transform.rotation;
  return pose;
}
 
geometry_msgs::msg::TransformStamped poseToTransformStamped(
  const geometry_msgs::msg::PoseStamped & pose, const std::string & child_frame)
{
  geometry_msgs::msg::TransformStamped transform;
  transform.header = pose.header;
  transform.child_frame_id = child_frame;
  transform.transform.translation.x = pose.pose.position.x;
  transform.transform.translation.y = pose.pose.position.y;
  transform.transform.translation.z = pose.pose.position.z;
  transform.transform.rotation = pose.pose.orientation;
  return transform;
}
 
bool getFreshPose(
  nav2::TransformBuffer & tf_buffer,
  const std::string & target_frame,
  const std::string & source_frame,
  const rclcpp::Time & current_time,
  double staleness_threshold,
  geometry_msgs::msg::PoseStamped & pose)
{
  geometry_msgs::msg::TransformStamped transform;
  if (lookupTransformWithStalenessCheck(
      tf_buffer, target_frame, source_frame, current_time, staleness_threshold, transform))
  {
    pose = transformToPoseStamped(transform);
    return true;
  }
  return false;
}
 
bool getCurrentPose(
  geometry_msgs::msg::PoseStamped & global_pose,
  nav2::TransformBuffer & tf_buffer, const std::string global_frame,
  const std::string robot_frame, const double transform_timeout,
  const rclcpp::Time stamp)
{
  tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
  global_pose.header.frame_id = robot_frame;
  global_pose.header.stamp = stamp;
 
  return transformPoseInTargetFrame(
    global_pose, global_pose, tf_buffer, global_frame, transform_timeout);
}
 
bool transformPoseInTargetFrame(
  const geometry_msgs::msg::PoseStamped & input_pose,
  geometry_msgs::msg::PoseStamped & transformed_pose,
  nav2::TransformBuffer & tf_buffer, const std::string target_frame,
  const double transform_timeout)
{
  static rclcpp::Logger logger = rclcpp::get_logger("transformPoseInTargetFrame");
 
  if (input_pose.header.frame_id == target_frame) {
    transformed_pose = input_pose;
    return true;
  }
 
  try {
    transformed_pose = tf_buffer.transform(
      input_pose, target_frame,
      tf2::durationFromSec(transform_timeout));
    return true;
  } catch (tf2::LookupException & ex) {
    RCLCPP_ERROR(
      logger,
      "No Transform available Error looking up target frame: %s\n", ex.what());
  } catch (tf2::ConnectivityException & ex) {
    RCLCPP_ERROR(
      logger,
      "Connectivity Error looking up target frame: %s\n", ex.what());
  } catch (tf2::ExtrapolationException & ex) {
    RCLCPP_ERROR(
      logger,
      "Extrapolation Error looking up target frame: %s\n", ex.what());
  } catch (tf2::TimeoutException & ex) {
    RCLCPP_ERROR(
      logger,
      "Transform timeout with tolerance: %.4f", transform_timeout);
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(
      logger, "Failed to transform from %s to %s",
      input_pose.header.frame_id.c_str(), target_frame.c_str());
  }
 
  return false;
}
 
bool getTransform(
  const std::string & source_frame_id,
  const std::string & target_frame_id,
  const tf2::Duration & transform_tolerance,
  const nav2::TransformBuffer::SharedPtr tf_buffer,
  geometry_msgs::msg::TransformStamped & transform_msg)
{
  if (source_frame_id == target_frame_id) {
    // We are already in required frame
    return true;
  }
 
  try {
    // Obtaining the transform to get data from source to target frame
    transform_msg = tf_buffer->lookupTransform(
      target_frame_id, source_frame_id,
      tf2::TimePointZero, transform_tolerance);
  } catch (tf2::TransformException & e) {
    RCLCPP_ERROR(
      rclcpp::get_logger("getTransform"),
      "Failed to get \"%s\"->\"%s\" frame transform: %s",
      source_frame_id.c_str(), target_frame_id.c_str(), e.what());
    return false;
  }
  return true;
}
 
bool getTransform(
  const std::string & source_frame_id,
  const std::string & target_frame_id,
  const tf2::Duration & transform_tolerance,
  const nav2::TransformBuffer::SharedPtr tf_buffer,
  tf2::Transform & tf2_transform)
{
  tf2_transform.setIdentity();  // initialize by identical transform
  geometry_msgs::msg::TransformStamped transform;
  if (getTransform(source_frame_id, target_frame_id, transform_tolerance, tf_buffer, transform)) {
    // Convert TransformStamped to TF2 transform
    tf2::fromMsg(transform.transform, tf2_transform);
    return true;
  }
  return false;
}
 
bool getTransform(
  const std::string & source_frame_id,
  const rclcpp::Time & source_time,
  const std::string & target_frame_id,
  const rclcpp::Time & target_time,
  const std::string & fixed_frame_id,
  const tf2::Duration & transform_tolerance,
  const nav2::TransformBuffer::SharedPtr tf_buffer,
  geometry_msgs::msg::TransformStamped & transform_msg)
{
  try {
    // Obtaining the transform to get data from source to target frame.
    // This also considers the time shift between source and target.
    transform_msg = tf_buffer->lookupTransform(
      target_frame_id, target_time,
      source_frame_id, source_time,
      fixed_frame_id, transform_tolerance);
  } catch (tf2::TransformException & ex) {
    RCLCPP_ERROR(
      rclcpp::get_logger("getTransform"),
      "Failed to get \"%s\"->\"%s\" frame transform: %s",
      source_frame_id.c_str(), target_frame_id.c_str(), ex.what());
    return false;
  }
 
  return true;
}
 
bool getTransform(
  const std::string & source_frame_id,
  const rclcpp::Time & source_time,
  const std::string & target_frame_id,
  const rclcpp::Time & target_time,
  const std::string & fixed_frame_id,
  const tf2::Duration & transform_tolerance,
  const nav2::TransformBuffer::SharedPtr tf_buffer,
  tf2::Transform & tf2_transform)
{
  geometry_msgs::msg::TransformStamped transform;
  tf2_transform.setIdentity();  // initialize by identical transform
  if (getTransform(
      source_frame_id, source_time, target_frame_id, target_time, fixed_frame_id,
      transform_tolerance, tf_buffer, transform))
  {
    // Convert TransformStamped to TF2 transform
    tf2::fromMsg(transform.transform, tf2_transform);
    return true;
  }
 
  return false;
}
 
bool validateTwist(const geometry_msgs::msg::Twist & msg)
{
  if (std::isinf(msg.linear.x) || std::isnan(msg.linear.x)) {
    return false;
  }
 
  if (std::isinf(msg.linear.y) || std::isnan(msg.linear.y)) {
    return false;
  }
 
  if (std::isinf(msg.linear.z) || std::isnan(msg.linear.z)) {
    return false;
  }
 
  if (std::isinf(msg.angular.x) || std::isnan(msg.angular.x)) {
    return false;
  }
 
  if (std::isinf(msg.angular.y) || std::isnan(msg.angular.y)) {
    return false;
  }
 
  if (std::isinf(msg.angular.z) || std::isnan(msg.angular.z)) {
    return false;
  }
 
  return true;
}
 
}  // end namespace nav2_util