19#include <rclcpp/rclcpp.hpp>
21#include <boost/uuid/uuid_generators.hpp>
22#include <boost/uuid/uuid_io.hpp>
23#include <boost/geometry.hpp>
24#include <boost/shared_ptr.hpp>
25#include <lanelet2_core/geometry/Point.h>
26#include <lanelet2_core/primitives/Lanelet.h>
27#include <lanelet2_core/geometry/LineString.h>
28#include <carma_planning_msgs/msg/trajectory_plan_point.hpp>
29#include <carma_planning_msgs/msg/trajectory_plan.hpp>
30#include <carma_planning_msgs/srv/plan_trajectory.hpp>
31#include <carma_planning_msgs/msg/maneuver.hpp>
32#include <autoware_msgs/msg/lane.hpp>
33#include <carma_debug_ros2_msgs/msg/trajectory_curvature_speeds.hpp>
62#define GET_MANEUVER_PROPERTY(mvr, property)\
63 (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
64 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
65 ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
66 ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
67 throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))
74 using DebugPublisher = std::function<void(
const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds&)>;
115 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode>
nh_;
135 carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory>
yield_client_;
149 carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds
debug_msg_;
178 void applyOptimizedTargetSpeedProfile(
const carma_planning_msgs::msg::Maneuver& maneuver,
const double starting_speed, std::vector<PointSpeedPair>& points_and_target_speeds);
194 carma_planning_msgs::msg::VehicleState &ending_state_before_buffer,
const carma_planning_msgs::msg::VehicleState& state,
230 const rclcpp::Time& current_time);
242 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
243 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
258 const std::vector<carma_planning_msgs::msg::Maneuver>& maneuver_plan,
259 const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr& req,
260 std::vector<double>& final_speeds);
276 double min_remaining_time_seconds = 0.0)
const;
293 const std::string& plugin_name,
294 std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh);
302 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
303 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp);
310 void set_yield_client(carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> client);
Class containing primary business logic for the Light Controlled Intersection Tactical Plugin.
carma_planning_msgs::msg::TrajectoryPlan last_trajectory_time_unbound_
carma_planning_msgs::msg::VehicleState ending_state_before_buffer_
carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > yield_client_
void planTrajectoryCB(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
Function to process the light controlled intersection tactical plugin service call for trajectory pla...
FRIEND_TEST(LCITacticalPluginTest, planTrajectoryCB)
uint32_t scheduled_enter_time_
carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds debug_msg_
void applyTrajectorySmoothingAlgorithm(const carma_wm::WorldModelConstPtr &wm, std::vector< PointSpeedPair > &points_and_target_speeds, double start_dist, double remaining_dist, double starting_speed, double departure_speed, TrajectoryParams tsp)
Creates a speed profile according to case one or two of the light controlled intersection,...
FRIEND_TEST(LCITacticalPluginTest, applyOptimizedTargetSpeedProfile)
std::string light_controlled_intersection_strategy_
rclcpp::Time latest_traj_request_header_stamp_
double last_successful_scheduled_entry_time_
double current_downtrack_
FRIEND_TEST(LCITacticalPluginTest, createGeometryProfile)
boost::optional< bool > is_last_case_successful_
std::vector< double > last_speeds_time_unbound_
DebugPublisher debug_publisher_
uint32_t scheduled_stop_time_
bool isLastTrajectoryValid(const rclcpp::Time ¤t_time, double min_remaining_time_seconds=0.0) const
Checks if the last trajectory plan remains valid based on the current time.
uint32_t scheduled_depart_time_
void applyOptimizedTargetSpeedProfile(const carma_planning_msgs::msg::Maneuver &maneuver, const double starting_speed, std::vector< PointSpeedPair > &points_and_target_speeds)
Apply optimized target speeds to the trajectory determined for fixed-time and actuated signals....
void planTrajectorySmoothing(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
Smooths the trajectory as part of the trajectory planning process.
carma_planning_msgs::msg::TrajectoryPlan generateNewTrajectory(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuver_plan, const carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr &req, std::vector< double > &final_speeds)
Generates a new trajectory plan based on the provided maneuver plan and request. NOTE: This function ...
FRIEND_TEST(LCITacticalPluginTest, planTrajectorySmoothing)
uint32_t street_msg_timestamp_
void setConfig(const Config &config)
Setter function to set a new config for this object.
void logDebugInfoAboutPreviousTrajectory()
Logs debug information about the previously planned trajectory.
LightControlledIntersectionTacticalPlugin(carma_wm::WorldModelConstPtr wm, const Config &config, const DebugPublisher &debug_publisher, const std::string &plugin_name, std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh)
LightControlledIntersectionTacticalPlugin constructor.
const std::string LCI_TACTICAL_LOGGER
double findSpeedLimit(const lanelet::ConstLanelet &llt, const carma_wm::WorldModelConstPtr &wm) const
Given a Lanelet, find its associated Speed Limit.
std::shared_ptr< carma_ros2_utils::CarmaLifecycleNode > nh_
FRIEND_TEST(LCITacticalPluginTest, applyTrajectorySmoothingAlgorithm)
FRIEND_TEST(LCITacticalPluginTest, setConfig)
void set_yield_client(carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > client)
set the yield service
bool shouldUseLastTrajectory(TSCase new_case, bool is_new_case_successful, const rclcpp::Time ¤t_time)
Determines whether the last trajectory should be reused based on the planning case....
carma_wm::WorldModelConstPtr wm_
uint32_t scheduled_latest_depart_time_
boost::optional< TSCase > last_case_
double last_successful_ending_downtrack_
std::vector< PointSpeedPair > createGeometryProfile(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
Creates geometry profile to return a point speed pair struct for INTERSECTION_TRANSIT maneuver types ...
std::shared_ptr< const WorldModel > WorldModelConstPtr
basic_autonomy::waypoint_generation::PointSpeedPair PointSpeedPair
std::function< void(const carma_debug_ros2_msgs::msg::TrajectoryCurvatureSpeeds &)> DebugPublisher
basic_autonomy::waypoint_generation::DetailedTrajConfig DetailedTrajConfig
basic_autonomy::waypoint_generation::GeneralTrajConfig GeneralTrajConfig
Stuct containing the algorithm configuration values for light_controlled_intersection_tactical_plugin...