LocalizationManager.hpp

Go to the documentation of this file.

/*
 * Copyright (C) 2022 LEIDOS.
 *
 * 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 <rclcpp/rclcpp.hpp>
#include <boost/shared_ptr.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <boost/optional.hpp>
#include <autoware_msgs/msg/ndt_stat.hpp>
#include <tf2/LinearMath/Vector3.h>
#include <carma_msgs/msg/system_alert.hpp>
#include <carma_localization_msgs/msg/localization_status_report.hpp>
#include <functional>
#include <unordered_set>
#include <carma_ros2_utils/timers/Timer.hpp>
#include <carma_ros2_utils/timers/TimerFactory.hpp>
#include <carma_ros2_utils/timers/ROSTimerFactory.hpp>
#include "localization_manager/LocalizationTypes.hpp"
#include "localization_manager/LocalizationManagerConfig.hpp"
#include "localization_manager/LocalizationTransitionTable.hpp"
 
namespace localization_manager
{
    class LocalizationManager
    {
 
    public:
        using PosePublisher = std::function<void(const geometry_msgs::msg::PoseStamped &)>;
        using ManagedInitialPosePublisher = std::function<void(const geometry_msgs::msg::PoseWithCovarianceStamped &)>;
        using StatePublisher = std::function<void(const carma_localization_msgs::msg::LocalizationStatusReport &)>;
        using TimerUniquePtr = std::unique_ptr<carma_ros2_utils::timers::Timer>;
 
        LocalizationManager(PosePublisher pose_pub, StatePublisher state_pub,
                            ManagedInitialPosePublisher initialpose_pub,
                            const LocalizationManagerConfig &config,
                            rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger,
                            std::unique_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory);
 
        void gnssPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
 
        void poseAndStatsCallback(const geometry_msgs::msg::PoseStamped::ConstPtr pose,
                                  const autoware_msgs::msg::NDTStat::ConstPtr stats);
 
        void systemAlertCallback(const carma_msgs::msg::SystemAlert::SharedPtr alert);
 
        void initialPoseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
 
        void posePubTick();
 
        void stateTransitionCallback(LocalizationState prev_state, LocalizationState new_state, LocalizationSignal signal);
 
        void timerCallback(const LocalizationState origin_state);
 
        LocalizationState getState() const;
 
        void clearTimers();
 
        void setConfig(const LocalizationManagerConfig& config);
 
    private:
        static const std::unordered_set<std::string> LIDAR_FAILURE_STRINGS; // Static const container defined in cpp file
 
        PosePublisher pose_pub_;
        StatePublisher state_pub_;
        ManagedInitialPosePublisher initialpose_pub_;
 
        LocalizationManagerConfig config_;
 
        LocalizationTransitionTable transition_table_;
 
        boost::optional<rclcpp::Time> prev_ndt_stamp_;
 
        int lidar_init_sequential_timesteps_counter_ = 0;
        bool is_sequential_ = false;
        boost::optional<geometry_msgs::msg::PoseStamped> last_raw_gnss_value_;
        boost::optional<tf2::Vector3> gnss_offset_;
        boost::optional<geometry_msgs::msg::PoseStamped> current_pose_;
 
        // Logger interface
        rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
 
        // Using timer factory
        std::mutex mutex_;
        std::unique_ptr<carma_ros2_utils::timers::TimerFactory> timer_factory_;
        std::unordered_map<uint32_t, std::pair<TimerUniquePtr, bool>> timers_;
        TimerUniquePtr current_timer_;
        uint32_t next_id_ = 0; // Timer id counter
        uint32_t current_timer_id_;
        rcl_clock_type_t timer_clock_type_ = RCL_SYSTEM_TIME;
 
        double computeNDTFreq(const rclcpp::Time &new_stamp);
 
        double computeFreq(const rclcpp::Time &old_stamp, const rclcpp::Time &new_stamp) const;
 
        uint32_t nextId();
    };
}