WMListener.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 <rclcpp/rclcpp.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_ros2_utils/carma_ros2_utils.hpp>
#include <autoware_lanelet2_msgs/msg/map_bin.hpp>
#include <queue>
#include <carma_perception_msgs/msg/roadway_obstacle_list.hpp>
#include <carma_planning_msgs/msg/route.hpp>
#include <carma_v2x_msgs/msg/spat.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <rosgraph_msgs/msg/clock.hpp>
 
namespace carma_wm
{
class WMListenerWorker;  // Forward declaration of worker class
 
class WMListener
{
public:
  WMListener(
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base,
    rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging,
    rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics,
    rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_,
    bool multi_thread = false);
 
  ~WMListener();
 
  WorldModelConstPtr getWorldModel();
 
  void setMapCallback(std::function<void()> callback);
 
  void setRouteCallback(std::function<void()> callback);
 
  std::unique_lock<std::mutex> getLock(bool pre_locked = true);
 
  void setConfigSpeedLimit(double config_lim) const;
 
  void enableUpdatesWithoutRouteWL();
 
  bool checkIfReRoutingNeededWL();
 
 
private:
  // Callback function that uses lock to edit the map
  void mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg);
  carma_ros2_utils::SubPtr<carma_perception_msgs::msg::RoadwayObstacleList> roadway_objects_sub_;
  carma_ros2_utils::SubPtr<autoware_lanelet2_msgs::msg::MapBin> map_update_sub_;
 
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_;
  rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logging_;
  rclcpp::node_interfaces::NodeTopicsInterface::SharedPtr node_topics_;
  rclcpp::node_interfaces::NodeParametersInterface::SharedPtr node_params_;
 
  std::unique_ptr<WMListenerWorker> worker_;
 
  carma_ros2_utils::SubPtr<autoware_lanelet2_msgs::msg::MapBin> map_sub_;
  carma_ros2_utils::SubPtr<carma_planning_msgs::msg::Route> route_sub_;
  carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::SPAT> traffic_spat_sub_;
  carma_ros2_utils::SubPtr<rosgraph_msgs::msg::Clock> sim_clock_sub_;
  carma_ros2_utils::SubPtr<rosgraph_msgs::msg::Clock> ros1_clock_sub_;
  const bool multi_threaded_;
  std::mutex mw_mutex_;
 
 
};
}  // namespace carma_wm