GeofenceScheduler.hpp

Go to the documentation of this file.

#pragma once
/*
 * 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 <functional>
#include <mutex>
#include <memory>
#include <unordered_map>
#include <carma_wm_ctrl/Geofence.hpp>
#include <carma_ros2_utils/timers/Timer.hpp>
#include <carma_ros2_utils/timers/TimerFactory.hpp>
#include <carma_ros2_utils/timers/ROSTimerFactory.hpp>
#include <gtest/gtest_prod.h>
 
 
namespace carma_wm_ctrl
{
class GeofenceScheduler
{
  using Timer = carma_ros2_utils::timers::Timer;
  using TimerFactory = carma_ros2_utils::timers::TimerFactory;
  using ROSTimerFactory = carma_ros2_utils::timers::ROSTimerFactory;
  using TimerPtr = std::unique_ptr<Timer>;
 
  std::mutex mutex_;
  std::shared_ptr<TimerFactory> timerFactory_;
  std::unordered_map<uint32_t, std::pair<TimerPtr, bool>> timers_;  // Pairing of timers with their Id and valid status
  std::unique_ptr<Timer> deletion_timer_;
  std::function<void(std::shared_ptr<Geofence>)> active_callback_;
  std::function<void(std::shared_ptr<Geofence>)> inactive_callback_;
  uint32_t next_id_ = 0;  // Timer id counter
  rcl_clock_type_t clock_type_ = RCL_SYSTEM_TIME;
 
public:
  GeofenceScheduler(std::shared_ptr<TimerFactory> timerFactory);
 
  void addGeofence(std::shared_ptr<Geofence> gf_ptr);
 
  void onGeofenceActive(std::function<void(std::shared_ptr<Geofence>)> active_callback);
  void onGeofenceInactive(std::function<void(std::shared_ptr<Geofence>)> inactive_callback);
 
  void clearTimers();
 
  rcl_clock_type_t getClockType();
 
  rclcpp::Time now();
  
private:
  uint32_t nextId();
 
  void startGeofenceCallback(std::shared_ptr<Geofence> gf_ptr, const unsigned int schedule_id, const int32_t timer_id);
  void endGeofenceCallback(std::shared_ptr<Geofence> gf_ptr, const unsigned int schedule_id, const int32_t timer_id);
 
};
}  // namespace carma_wm_ctrl