sci_strategic_plugin.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 <rclcpp/rclcpp.hpp>
#include <string>
#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
#include <carma_planning_msgs/msg/plugin.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <carma_v2x_msgs/msg/mobility_operation.hpp>
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <bsm_helper/bsm_helper.h>
#include <carma_wm/Geometry.hpp>
#include <lanelet2_core/Forward.h>
#include <gtest/gtest_prod.h>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <carma_v2x_msgs/msg/bsm.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string/classification.hpp>
#include <carma_guidance_plugins/strategic_plugin.hpp>
#include "sci_strategic_plugin_config.hpp"
 
namespace sci_strategic_plugin
{
enum TurnDirection {
          Straight,
          Right,
          Left
};
 
class SCIStrategicPlugin : public carma_guidance_plugins::StrategicPlugin
{
public:
  struct VehicleState
  {
    rclcpp::Time stamp;      // Timestamp of this state data
    double downtrack;     // The downtrack of the vehicle along the route at time stamp
    double speed;         // The speed of the vehicle at time stamp
    lanelet::Id lane_id;  // The current lane id of the vehicle at time stamp
  };
 
  
  explicit SCIStrategicPlugin(const rclcpp::NodeOptions &);
 
  void publishMobilityOperation();
 
  void plan_maneuvers_callback(
    std::shared_ptr<rmw_request_id_t> srv_header, 
    carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, 
    carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp);
 
  rcl_interfaces::msg::SetParametersResult 
  parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);
 
  void mobilityOperationCb(carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg);
 
  void currentPoseCb(geometry_msgs::msg::PoseStamped::UniquePtr msg);
 
  carma_planning_msgs::msg::Maneuver composeLaneFollowingManeuverMessage(int case_num, double start_dist, double end_dist, double start_speed,
                                                         double target_speed, rclcpp::Time start_time, double time_to_stop,
                                                         std::vector<lanelet::Id> lane_ids);
 
  carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double current_dist, double end_dist, double start_speed,
                                                      const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id, 
                                                      double stopping_accel, rclcpp::Time start_time, rclcpp::Time end_time) const;
 
  carma_planning_msgs::msg::Maneuver composeIntersectionTransitMessage(double start_dist, double end_dist, double start_speed, 
                                                      double target_speed, rclcpp::Time start_time, rclcpp::Time end_time, TurnDirection turn_direction,
                                                      const lanelet::Id& starting_lane_id, const lanelet::Id& ending_lane_id) const;
 
  VehicleState extractInitialState(const carma_planning_msgs::srv::PlanManeuvers::Request& req) const;
 
  std::vector<lanelet::ConstLanelet> getLaneletsBetweenWithException(double start_downtrack, double end_downtrack,
                                                                     bool shortest_path_only = false,
                                                                     bool bounds_inclusive = true) const;
 
 
  double findSpeedLimit(const lanelet::ConstLanelet& llt) const;
 
 
  int determine_speed_profile_case(double stop_dist, double current_speed, double schedule_stop_time, double speed_limit);
 
  double calc_speed_before_decel(double stop_time, double stop_dist, double current_speed) const;
 
  void parseStrategyParams(const std::string& strategy_params);
 
  double calcEstimatedStopTime(double stop_dist, double current_speed) const;
 
  void caseOneSpeedProfile(double speed_before_decel, double current_speed, double stop_time, std::vector<double>* float_metadata_list) const;
 
  void caseTwoSpeedProfile(double stop_dist, double speed_before_decel, double current_speed, double stop_time,  double speed_limit, std::vector<double>* float_metadata_list) const;
 
  double caseThreeSpeedProfile(double stop_dist, double current_speed, double stop_time) const;
 
  carma_v2x_msgs::msg::MobilityOperation generateMobilityOperation();
 
  void BSMCb(carma_v2x_msgs::msg::BSM::UniquePtr msg);
 
  void guidance_state_cb(const carma_planning_msgs::msg::GuidanceState::UniquePtr msg);
 
  TurnDirection getTurnDirectionAtIntersection(std::vector<lanelet::ConstLanelet> lanelets_list);
  
  carma_ros2_utils::CallbackReturn on_configure_plugin();
  carma_ros2_utils::CallbackReturn on_activate_plugin();
 
  bool get_availability();
  std::string get_version_id();
        
 
 
  // CARMA Streets Variakes
  // timestamp for msg received from carma streets
  unsigned long long street_msg_timestamp_ = 0;
  // scheduled stop time
  unsigned long long scheduled_stop_time_ = 0;
  // scheduled enter time
  unsigned long long scheduled_enter_time_ = 0;
  // scheduled depart time
  unsigned long long scheduled_depart_time_ = 0;
  // scheduled latest depart position
  uint32_t scheduled_departure_position_ = std::numeric_limits<uint32_t>::max();
  // flag to show if the vehicle is allowed in intersection
  bool is_allowed_int_ = false;
 
  TurnDirection intersection_turn_direction_ = TurnDirection::Straight;
  bool vehicle_engaged_ = false;
 
  // approximate speed limit 
  double speed_limit_ = 100.0;
 
  // downtrack of host vehicle
  double current_downtrack_ = 0.0;
 
  bool approaching_stop_controlled_interction_ = false;
 
  std::string bsm_id_ = "default_bsm_id";
  uint8_t bsm_msg_count_ = 0;
  uint16_t bsm_sec_mark_ = 0;
 
  private:
 
  carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::MobilityOperation> mob_operation_sub_;
  carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> current_pose_sub_;
  carma_ros2_utils::SubPtr<carma_v2x_msgs::msg::BSM> bsm_sub_;
  carma_ros2_utils::PubPtr<carma_v2x_msgs::msg::MobilityOperation> mobility_operation_pub_;
  carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> guidance_state_sub_;
 
  bool guidance_engaged_ = false;
 
  // timer to publish mobility operation message
  rclcpp::TimerBase::SharedPtr mob_op_pub_timer_;
 
  carma_wm::WorldModelConstPtr wm_;
 
  SCIStrategicPluginConfig config_;
 
  boost::optional<double> intersection_speed_;
  boost::optional<double> intersection_end_downtrack_;
 
  // strategy for stop controlled intersection
  std::string stop_controlled_intersection_strategy_ = "Carma/stop_controlled_intersection";
  std::string previous_strategy_params_ = "";  
  
  // Unit test helper functions
  carma_wm::WorldModelConstPtr get_wm() { return wm_; }
  void set_wm(carma_wm::WorldModelConstPtr new_wm) { wm_ = new_wm; }
 
  // Unit Test Accessors
  FRIEND_TEST(SCIStrategicPluginTest, findSpeedLimit);
  FRIEND_TEST(SCIStrategicPluginTest, moboperationcbtest);
  FRIEND_TEST(SCIStrategicPluginTest, parseStrategyParamstest);
  FRIEND_TEST(SCIStrategicPluginTest, calcEstimatedStopTimetest);
  FRIEND_TEST(SCIStrategicPluginTest, calc_speed_before_deceltest);
  FRIEND_TEST(SCIStrategicPluginTest, determine_speed_profile_casetest);
  FRIEND_TEST(SCIStrategicPluginTest, caseOneSpeedProfiletest);
  FRIEND_TEST(SCIStrategicPluginTest, caseTwoSpeedProfiletest);
  FRIEND_TEST(SCIStrategicPluginTest, caseThreeSpeedProfiletest);
  FRIEND_TEST(SCIStrategicPluginTest, testIntersectionturndirection);
  FRIEND_TEST(SCIStrategicPluginTest, DISABLED_maneuvercbtest);
  FRIEND_TEST(SCIStrategicPluginTest, maneuvercbtest);
};
}  // namespace sci_strategic_plugin