yield_plugin.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2022-2026 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 <vector>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
#include <carma_planning_msgs/msg/plugin.hpp>
#include <carma_v2x_msgs/msg/mobility_request.hpp>
#include <carma_v2x_msgs/msg/mobility_response.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <carma_planning_msgs/msg/lane_change_status.hpp>
#include <boost/shared_ptr.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <boost/geometry.hpp>
#include <carma_wm/WMListener.hpp>
#include <functional>
#include "yield_config.hpp"
#include <unordered_set>
#include <carma_planning_msgs/srv/plan_trajectory.hpp>
#include <std_msgs/msg/string.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <trajectory_utils/quintic_coefficient_calculator.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <carma_v2x_msgs/msg/location_ecef.hpp>
#include <carma_v2x_msgs/msg/trajectory.hpp>
#include <carma_v2x_msgs/msg/plan_type.hpp>
#include <carma_wm/Geometry.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_wm/collision_detection.hpp>
#include <carma_wm/TrafficControl.hpp>
 
 
namespace yield_plugin
{
using MobilityResponseCB = std::function<void(const carma_v2x_msgs::msg::MobilityResponse&)>;
using LaneChangeStatusCB = std::function<void(const carma_planning_msgs::msg::LaneChangeStatus&)>;
 
struct PointSpeedPair
{
  lanelet::BasicPoint2d point;
  double speed = 0;
};
 
struct GetCollisionResult
{
  rclcpp::Time collision_time;
  lanelet::BasicPoint2d ego_point;
  lanelet::BasicPoint2d object_point;
};
 
class YieldPlugin
{
public:
  YieldPlugin(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh, carma_wm::WorldModelConstPtr wm, YieldPluginConfig config,
                       MobilityResponseCB mobility_response_publisher,
                       LaneChangeStatusCB lc_status_publisher);
 
  void plan_trajectory_callback(
    carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
    carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
 
  carma_planning_msgs::msg::TrajectoryPlan update_traj_for_object(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double initial_velocity);
 
  double polynomial_calc(std::vector<double> coeff, double x) const;
 
  double polynomial_calc_d(std::vector<double> coeff, double x) const;
 
  double max_trajectory_speed(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory_points, double timestamp_in_sec_to_search_until) const;
 
  std::vector<double> get_relative_downtracks(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const;
 
  void mobilityrequest_cb(const carma_v2x_msgs::msg::MobilityRequest::UniquePtr msg);
 
  void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
  std::vector<lanelet::BasicPoint2d> convert_eceftrajectory_to_mappoints(const carma_v2x_msgs::msg::Trajectory& ecef_trajectory) const;
 
  lanelet::BasicPoint2d ecef_to_map_point(const carma_v2x_msgs::msg::LocationECEF& ecef_point) const;
 
  carma_v2x_msgs::msg::MobilityResponse compose_mobility_response(const std::string& resp_recipient_id, const std::string& req_plan_id, bool response) const;
 
  carma_planning_msgs::msg::TrajectoryPlan generate_JMT_trajectory(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double initial_pos, double goal_pos,
    double initial_velocity, double goal_velocity, double planning_time, double original_max_speed);
 
  carma_planning_msgs::msg::TrajectoryPlan update_traj_for_cooperative_behavior(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double current_speed);
 
  std::vector<std::pair<int, lanelet::BasicPoint2d>> detect_trajectories_intersection(std::vector<lanelet::BasicPoint2d> self_trajectory, std::vector<lanelet::BasicPoint2d> incoming_trajectory) const;
 
 
  void set_incoming_request_info(std::vector <lanelet::BasicPoint2d> req_trajectory, double req_speed, double req_planning_time, double req_timestamp);
 
  void lookup_ecef_to_map_transform();
 
  double check_traj_for_digital_min_gap(const carma_planning_msgs::msg::TrajectoryPlan& original_tp) const;
 
  void set_georeference_string(const std::string& georeference);
 
  void set_external_objects(const std::vector<carma_perception_msgs::msg::ExternalObject>& object_list);
 
  void update_route_llt_cache();
 
  std::optional<GetCollisionResult> get_collision(const carma_planning_msgs::msg::TrajectoryPlan& ego_trajectory, const std::vector<carma_perception_msgs::msg::PredictedState>& object_predictions, double collision_radius, double ego_max_speed);
 
  std::optional<rclcpp::Time> get_collision_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const carma_perception_msgs::msg::ExternalObject& curr_obstacle, double original_tp_max_speed);
 
  bool is_object_behind_vehicle(uint32_t object_id, const rclcpp::Time& collision_time, double vehicle_point, double object_downtrack);
 
  std::optional<std::pair<carma_perception_msgs::msg::ExternalObject, double>> get_earliest_collision_object_and_time(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects);
 
  std::unordered_map<uint32_t, rclcpp::Time> get_collision_times_concurrently(const carma_planning_msgs::msg::TrajectoryPlan& original_tp, const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects, double original_tp_max_speed);
 
private:
  std::unordered_map<uint32_t, rclcpp::Time> get_collision_times_concurrently_cpu(
    const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
    const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects,
    double original_tp_max_speed);
 
  std::unordered_map<uint32_t, rclcpp::Time> get_collision_times_concurrently_cuda(
    const carma_planning_msgs::msg::TrajectoryPlan& original_tp,
    const std::vector<carma_perception_msgs::msg::ExternalObject>& external_objects,
    double original_tp_max_speed);
 
  std::pair<bool, int> find_on_route_in_predictions(
    const std::vector<carma_perception_msgs::msg::PredictedState>& predictions,
    int stride, bool zero_speed) const;
 
public:
  double get_predicted_velocity_at_time(const geometry_msgs::msg::Twist& object_velocity_in_map_frame, const carma_planning_msgs::msg::TrajectoryPlan& original_tp, double timestamp_in_sec_to_predict);
 
 
private:
 
  carma_wm::WorldModelConstPtr wm_;
  YieldPluginConfig config_;
  MobilityResponseCB mobility_response_publisher_;
  LaneChangeStatusCB lc_status_publisher_;
  std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh_;
  std::set<lanelet::Id> route_llt_ids_;
  // Store lanelet polygons for precise geometric containment checking instead of just bounding boxes
  std::vector<lanelet::ConstLanelet> route_llt_polygons_;
  lanelet::Id previous_llt_id_;
  std::vector<carma_perception_msgs::msg::ExternalObject> external_objects_;
  std::unordered_map<uint32_t, int> consecutive_clearance_count_for_obstacles_;
  // flag to show if it is possible for the vehicle to accept the cooperative request
  bool cooperative_request_acceptable_ = false;
 
  // incoming request trajectory information:
  std::vector <lanelet::BasicPoint2d> req_trajectory_points_;
  double req_target_speed_ = 0;
  double req_timestamp_ = 0;
  double req_target_plan_time_ = 0;
  int timesteps_since_last_req_ = 0;
  int clc_urgency_ = 0;
  std::optional<double> last_speed_ = std::nullopt;
  std::optional<rclcpp::Time> last_speed_time_ = std::nullopt;
  std::optional<rclcpp::Time> first_time_stopped_to_prevent_collision_ = std::nullopt;
  std::optional<
    carma_planning_msgs::msg::TrajectoryPlan> last_traj_plan_committed_to_stopping_ = std::nullopt;
  // time between ecef trajectory points
  double ecef_traj_timestep_ = 0.1;
 
  geometry_msgs::msg::Vector3 host_vehicle_size;
  double current_speed_;
  // BSM Message
  std::string host_bsm_id_;
 
  std::string georeference_{""};
  std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;
 
  std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core)
  {
    std::string res = "";
    for (size_t i=0; i<bsm_core.id.size(); i++)
    {
      res+=std::to_string(bsm_core.id[i]);
    }
      return res;
  }
 
};
}  // namespace yield_plugin