.. _program_listing_file__tmp_ws_src_SMACC2_smacc2_client_library_nav2z_client_nav2z_client_include_nav2z_client_components_odom_tracker_odom_tracker.hpp:

Program Listing for File odom_tracker.hpp
=========================================

|exhale_lsh| :ref:`Return to documentation for file <file__tmp_ws_src_SMACC2_smacc2_client_library_nav2z_client_nav2z_client_include_nav2z_client_components_odom_tracker_odom_tracker.hpp>` (``/tmp/ws/src/SMACC2/smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/odom_tracker/odom_tracker.hpp``)

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

.. code-block:: cpp

   // 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
    *
    ******************************************************************************************************************/
   #pragma once
   
   #include <tf2/transform_datatypes.h>
   #include <smacc2/common.hpp>
   #include <smacc2/component.hpp>
   
   #include <rclcpp/rclcpp.hpp>
   
   #include <memory>
   #include <mutex>
   #include <vector>
   
   #include <geometry_msgs/msg/point.hpp>
   #include <geometry_msgs/msg/pose.hpp>
   #include <nav2_msgs/action/navigate_to_pose.hpp>
   #include <nav_msgs/msg/odometry.hpp>
   #include <nav_msgs/msg/path.hpp>
   
   #include <std_msgs/msg/header.hpp>
   
   namespace cl_nav2z
   {
   namespace odom_tracker
   {
   enum class WorkingMode : uint8_t
   {
     RECORD_PATH = 0,
     CLEAR_PATH = 1,
     IDLE = 2
   };
   
   class OdomTracker : public smacc2::ISmaccComponent
   {
   public:
     // by default, the component start in record_path mode and publishing the
     // current path
     OdomTracker(std::string odomtopicName = "/odom", std::string odomFrame = "odom");
   
     // threadsafe
     // The odom parameters is the main input of this tracker
     virtual void processOdometryMessage(const nav_msgs::msg::Odometry::SharedPtr odom);
   
     // ------ CONTROL COMMANDS ---------------------
     // threadsafe
     void setWorkingMode(WorkingMode workingMode);
   
     // threadsafe
     void setPublishMessages(bool value);
   
     // threadsafe
     void pushPath();
   
     // threadsafe
     void pushPath(std::string pathname);
   
     // threadsafe
     void popPath(int pathCount = 1, bool keepPreviousPath = false);
   
     // threadsafe
     void clearPath();
   
     // threadsafe
     void setStartPoint(const geometry_msgs::msg::PoseStamped & pose);
   
     // threadsafe
     void setStartPoint(const geometry_msgs::msg::Pose & pose);
   
     // threadsafe - use only for recording mode, it is reset always a new path is pushed, popped or cleared
     void setCurrentMotionGoal(const geometry_msgs::msg::PoseStamped & pose);
   
     void setCurrentPathName(const std::string & currentPathName);
   
     // threadsafe
     std::optional<geometry_msgs::msg::PoseStamped> getCurrentMotionGoal();
   
     // threadsafe
     nav_msgs::msg::Path getPath();
   
     void logStateString(bool debug = true);
   
   protected:
     void onInitialize() override;
   
     void updateConfiguration();
   
     virtual void rtPublishPaths(rclcpp::Time timestamp);
   
     // this is called when a new odom message is received in record path mode
     virtual bool updateRecordPath(const nav_msgs::msg::Odometry & odom);
   
     // this is called when a new odom message is received in clear path mode
     virtual bool updateClearPath(const nav_msgs::msg::Odometry & odom);
   
     void updateAggregatedStackPath();
   
     // -------------- OUTPUTS ---------------------
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathPub_;
     rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr robotBasePathStackedPub_;
   
     // --------------- INPUTS ------------------------
     // optional, this class can be used directly calling the odomProcessing method
     // without any subscriber
     rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odomSub_;
   
     // -------------- PARAMETERS ----------------------
     double recordPointDistanceThreshold_;
   
     double recordAngularDistanceThreshold_;
   
     double clearPointDistanceThreshold_;
   
     double clearAngularDistanceThreshold_;
   
     std::string odomFrame_;
   
     std::string odomTopicName_;
   
     // --------------- STATE ---------------
     // default true
     bool publishMessages;
   
     nav_msgs::msg::Path baseTrajectory_;
   
     WorkingMode workingMode_;
   
     std::vector<nav_msgs::msg::Path> pathStack_;
   
     struct PathInfo
     {
       std::string name;
       std::optional<geometry_msgs::msg::PoseStamped> goalPose;
     };
   
     std::vector<PathInfo> pathInfos_;
   
     nav_msgs::msg::Path aggregatedStackPathMsg_;
   
     // subscribes to topic on init if true
     bool subscribeToOdometryTopic_;
   
     std::optional<geometry_msgs::msg::PoseStamped> currentMotionGoal_;
     std::string currentPathName_;
   
     std::mutex m_mutex_;
   };
   
   inline double p2pDistance(
     const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
   {
     double dx = (p1.x - p2.x);
     double dy = (p1.y - p2.y);
     double dz = (p2.z - p2.z);
     double dist = sqrt(dx * dx + dy * dy + dz * dz);
     return dist;
   }
   }  // namespace odom_tracker
   }  // namespace cl_nav2z