WMListenerWorker.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2019-2021 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 <autoware_lanelet2_msgs/msg/map_bin.hpp>
#include <carma_planning_msgs/msg/route.hpp>
#include <carma_v2x_msgs/msg/spat.hpp>
#include <carma_wm/CARMAWorldModel.hpp>
#include <carma_wm/TrafficControl.hpp>
#include <queue>
#include <carma_wm/SignalizedIntersectionManager.hpp>
#include <utility>
#include <rosgraph_msgs/msg/clock.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace carma_wm
{
class WMListenerWorker
{
public:
  explicit WMListenerWorker(rclcpp::Logger logger = rclcpp::get_logger("carma_wm"));
 
  WorldModelConstPtr getWorldModel() const;
 
  void mapCallback(const autoware_lanelet2_msgs::msg::MapBin::SharedPtr map_msg);
 
  void mapUpdateCallback(autoware_lanelet2_msgs::msg::MapBin::SharedPtr geofence_msg);
 
  void routeCallback(const carma_planning_msgs::msg::Route::SharedPtr route_msg);
 
  void ros1ClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg);
 
  void simClockCallback(const rosgraph_msgs::msg::Clock::SharedPtr clock_msg);
 
  void roadwayObjectListCallback(const carma_perception_msgs::msg::RoadwayObstacleList::SharedPtr msg);
 
  void setMapCallback(std::function<void()> callback);
 
  void setRouteCallback(std::function<void()> callback);
 
  void setConfigSpeedLimit(double config_lim);
 
  double getConfigSpeedLimit() const;
 
 
  void setVehicleParticipationType(std::string participant);
 
  std::string getVehicleParticipationType() const;
 
 
  bool checkIfReRoutingNeeded() const;
  void enableUpdatesWithoutRoute();
 
  LaneletRoutingGraphPtr routingGraphFromMsg(const autoware_lanelet2_msgs::msg::RoutingGraph& msg, lanelet::LaneletMapPtr map) const;
 
  void incomingSpatCallback(const carma_v2x_msgs::msg::SPAT::SharedPtr spat_msg);
 
  void isUsingSimTime(bool use_sim_time) const;
 
  void isSpatWallTime(bool use_real_time_spat_in_sim) const;
 
  void setWMSpatProcessingState(const SIGNAL_PHASE_PROCESSING& phase_type) const;
 
  SIGNAL_PHASE_PROCESSING getWMSpatProcessingState() const;
 
private:
  rclcpp::Logger logger_;
  std::shared_ptr<CARMAWorldModel> world_model_;
  std::function<void()> map_callback_;
  std::function<void()> route_callback_;
  void newRegemUpdateHelper(lanelet::Lanelet parent_llt, lanelet::RegulatoryElement* regem) const;
  double config_speed_limit_;
 
  size_t current_map_version_ = 0; // Current map version based on recived map messages
  std::queue<autoware_lanelet2_msgs::msg::MapBin::SharedPtr> map_update_queue_; // Update queue used to cache map updates when they cannot be immeadiatly applied due to waiting for rerouting
  boost::optional<carma_planning_msgs::msg::Route> delayed_route_msg_;
 
  bool recompute_route_flag_=false; // indicates whether if this node should recompute its route based on invalidated msg
  bool rerouting_flag_=false; //indicates whether if route node is in middle of rerouting
  bool route_node_flag_=false; //indicates whether if this node is route node
  long most_recent_update_msg_seq_ = -1; // Tracks the current sequence number for map update messages. Dropping even a single message would invalidate the map
 
};
}  // namespace carma_wm