cp_pose.cpp

Go to the documentation of this file.

// Copyright 2021 RobosoftAI Inc.
//
// 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.
 
/*****************************************************************************************************************
 *
 *   Authors: Pablo Inigo Blasco, Brett Aldrich
 *
 ******************************************************************************************************************/
 
#include <nav2z_client/components/pose/cp_pose.hpp>
 
#include <memory>
#include <string>
 
namespace cl_nav2z
{
using namespace std::chrono_literals;
 
// static
std::shared_ptr<tf2_ros::TransformListener> Pose::tfListener_;
std::shared_ptr<tf2_ros::Buffer> Pose::tfBuffer_;
std::mutex Pose::listenerMutex_;
 
Pose::Pose(std::string targetFrame, std::string referenceFrame)
: isInitialized(false), poseFrameName_(targetFrame), referenceFrame_(referenceFrame)
 
{
  this->pose_.header.frame_id = referenceFrame_;
}
 
std::string referenceFrameToString(StandardReferenceFrames referenceFrame)
{
  switch (referenceFrame)
  {
    case StandardReferenceFrames::Map:
      return "map";
    case StandardReferenceFrames::Odometry:
      return "odom";
    default:
      return "odom";
  }
}
 
Pose::Pose(StandardReferenceFrames referenceFrame)
: Pose("base_link", referenceFrameToString(referenceFrame))
{
}
 
void Pose::onInitialize()
{
  RCLCPP_INFO(
    getLogger(), "[Pose] Creating Pose tracker component to track %s in the reference frame %s",
    poseFrameName_.c_str(), referenceFrame_.c_str());
 
  {
    // singleton
    std::lock_guard<std::mutex> guard(listenerMutex_);
    if (tfListener_ == nullptr)
    {
      tfBuffer_ = std::make_shared<tf2_ros::Buffer>(getNode()->get_clock());
      tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
    }
  }
}
 
void Pose::waitTransformUpdate(rclcpp::Rate r)
{
  bool found = false;
  RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate");
  while (rclcpp::ok() && !found)
  {
    tf2::Stamped<tf2::Transform> transform;
    try
    {
      {
        RCLCPP_INFO_THROTTLE(
          getLogger(), *(getNode()->get_clock()), 1000,
          "[Pose Component] waiting transform %s -> %s", referenceFrame_.c_str(),
          poseFrameName_.c_str());
        std::lock_guard<std::mutex> lock(listenerMutex_);
        auto transformstamped =
          tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, getNode()->now());
        tf2::fromMsg(transformstamped, transform);
      }
 
      {
        std::lock_guard<std::mutex> guard(m_mutex_);
        tf2::toMsg(transform, this->pose_.pose);
        this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
        this->pose_.header.frame_id = referenceFrame_;
        found = true;
        this->isInitialized = true;
      }
    }
    catch (tf2::TransformException & ex)
    {
      RCLCPP_ERROR_STREAM_THROTTLE(
        getLogger(), *(getNode()->get_clock()), 1000,
        "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
                             << "] ) is failing on pose update : " << ex.what());
    }
 
    r.sleep();
  }
  RCLCPP_INFO(getLogger(), "[Pose Component] waitTransformUpdate -> pose found!");
}
 
float Pose::getYaw() { return tf2::getYaw(pose_.pose.orientation); }
 
float Pose::getX() { return pose_.pose.position.x; }
float Pose::getY() { return pose_.pose.position.y; }
float Pose::getZ() { return pose_.pose.position.z; }
 
void Pose::update()
{
  tf2::Stamped<tf2::Transform> transform;
  try
  {
    if (!frozenReferenceFrameTime)
    {
      std::lock_guard<std::mutex> lock(listenerMutex_);
      RCLCPP_INFO(
        getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
        poseFrameName_.c_str());
      auto transformstamped =
        tfBuffer_->lookupTransform(referenceFrame_, poseFrameName_, rclcpp::Time(0));
      tf2::fromMsg(transformstamped, transform);
      RCLCPP_INFO(getLogger(), "[pose] transform found");
    }
    else
    {
      RCLCPP_INFO(
        getLogger(), "[pose] looking up transform: %s -> %s", referenceFrame_.c_str(),
        poseFrameName_.c_str());
      auto transformstamped = tfBuffer_->lookupTransform(
        referenceFrame_, *frozenReferenceFrameTime, poseFrameName_, rclcpp::Time(0),
        referenceFrame_, 1s);
      tf2::fromMsg(transformstamped, transform);
      RCLCPP_INFO(getLogger(), "[pose] transform found");
    }
 
    {
      std::lock_guard<std::mutex> guard(m_mutex_);
      tf2::toMsg(transform, this->pose_.pose);
      this->pose_.header.stamp = tf2_ros::toRclcpp(transform.stamp_);
      this->pose_.header.frame_id = referenceFrame_;
      this->isInitialized = true;
    }
  }
  catch (tf2::TransformException & ex)
  {
    // RCLCPP_DEBUG(getLogger(), "[pose] EXCEPTION");
    RCLCPP_ERROR_STREAM_THROTTLE(
      getLogger(), *(getNode()->get_clock()), 1000,
      "[Component pose] (" << poseFrameName_ << "/[" << referenceFrame_
                           << "] ) is failing on pose update : " << ex.what());
  }
}
}  // namespace cl_nav2z