.. _program_listing_file__tmp_ws_src_slam_toolbox_include_slam_toolbox_get_pose_helper.hpp:

Program Listing for File get_pose_helper.hpp
============================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_slam_toolbox_include_slam_toolbox_get_pose_helper.hpp>` (``/tmp/ws/src/slam_toolbox/include/slam_toolbox/get_pose_helper.hpp``)

.. |exhale_lsh| unicode:: U+021B0 .. UPWARDS ARROW WITH TIP LEFTWARDS

.. code-block:: cpp

   /*
    * snap_utils
    * Copyright (c) 2019, Samsung Research America
    *
    * THE WORK (AS DEFINED BELOW) IS PROVIDED UNDER THE TERMS OF THIS CREATIVE
    * COMMONS PUBLIC LICENSE ("CCPL" OR "LICENSE"). THE WORK IS PROTECTED BY
    * COPYRIGHT AND/OR OTHER APPLICABLE LAW. ANY USE OF THE WORK OTHER THAN AS
    * AUTHORIZED UNDER THIS LICENSE OR COPYRIGHT LAW IS PROHIBITED.
    *
    * BY EXERCISING ANY RIGHTS TO THE WORK PROVIDED HERE, YOU ACCEPT AND AGREE TO
    * BE BOUND BY THE TERMS OF THIS LICENSE. THE LICENSOR GRANTS YOU THE RIGHTS
    * CONTAINED HERE IN CONSIDERATION OF YOUR ACCEPTANCE OF SUCH TERMS AND
    * CONDITIONS.
    *
    */
   
   /* Author: Steven Macenski */
   
   #ifndef SLAM_TOOLBOX__GET_POSE_HELPER_HPP_
   #define SLAM_TOOLBOX__GET_POSE_HELPER_HPP_
   
   #include <string>
   #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
   #include "slam_toolbox/toolbox_types.hpp"
   #include "karto_sdk/Mapper.h"
   
   namespace pose_utils
   {
   
   // helper to get the robots position
   class GetPoseHelper
   {
   public:
     GetPoseHelper(
       tf2_ros::Buffer * tf,
       const std::string & base_frame,
       const std::string & odom_frame)
     : tf_(tf), base_frame_(base_frame), odom_frame_(odom_frame)
     {
     }
   
     bool getOdomPose(karto::Pose2 & karto_pose, const rclcpp::Time & t)
     {
       geometry_msgs::msg::TransformStamped base_ident, odom_pose;
       base_ident.header.stamp = t;
       base_ident.header.frame_id = base_frame_;
       base_ident.transform.rotation.w = 1.0;
   
       try {
         odom_pose = tf_->transform(base_ident, odom_frame_);
       } catch (tf2::TransformException & e) {
         return false;
       }
   
       const double yaw = tf2::getYaw(odom_pose.transform.rotation);
       karto_pose = karto::Pose2(odom_pose.transform.translation.x,
           odom_pose.transform.translation.y, yaw);
   
       return true;
     }
   
   private:
     tf2_ros::Buffer * tf_;
     std::string base_frame_, odom_frame_;
   };
   
   }  // namespace pose_utils
   
   #endif  // SLAM_TOOLBOX__GET_POSE_HELPER_HPP_