#include <plan_delegator.hpp>

Inheritance diagram for plan_delegator::PlanDelegator:

Collaboration diagram for plan_delegator::PlanDelegator:

Public Member Functions
	PlanDelegator (const rclcpp::NodeOptions &)
	PlanDelegator constructor. More...

void	maneuverPlanCallback (carma_planning_msgs::msg::ManeuverPlan::UniquePtr plan)
	Callback function of maneuver plan subscriber. More...

void	guidanceStateCallback (carma_planning_msgs::msg::GuidanceState::UniquePtr plan)
	Callback function of guidance state subscriber. More...

void	poseCallback (geometry_msgs::msg::PoseStamped::UniquePtr pose_msg)
	Callback function for vehicle pose subscriber. Updates latest_pose_ and makes calls to publishUpcomingLaneChangeStatus() and publishTurnSignalCommand(). More...

carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory >	getPlannerClientByName (const std::string &planner_name)
	Get PlanTrajectory service client by plugin name and create new PlanTrajectory service client if specified name does not exist. More...

bool	isManeuverExpired (const carma_planning_msgs::msg::Maneuver &maneuver, rclcpp::Time current_time) const
	Example if a maneuver end time has passed current system time. More...

std::shared_ptr< carma_planning_msgs::srv::PlanTrajectory::Request >	composePlanTrajectoryRequest (const carma_planning_msgs::msg::TrajectoryPlan &latest_trajectory_plan, const carma_planning_msgs::msg::ManeuverPlan &locked_maneuver_plan, const uint16_t &current_maneuver_index) const
	Generate new PlanTrajecory service request based on current planning progress. More...

void	lookupFrontBumperTransform ()
	Lookup transfrom from front bumper to base link. More...

void	updateManeuverParameters (carma_planning_msgs::msg::Maneuver &maneuver)
	Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver are shifted based on the distance between the base_link frame and the vehicle_front frame. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &)

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &)

Static Public Attributes
static const constexpr double	MILLISECOND_TO_SECOND = 0.001

Protected Attributes
Config	config_

std::unordered_map< std::string, carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > >	trajectory_planners_

carma_planning_msgs::msg::ManeuverPlan	latest_maneuver_plan_

bool	received_maneuver_plan_ = false

geometry_msgs::msg::PoseStamped	latest_pose_

geometry_msgs::msg::TwistStamped	latest_twist_

carma_wm::WMListener	wml_

carma_wm::WorldModelConstPtr	wm_

Private Member Functions
void	onTrajPlanTick ()
	Callback function for triggering trajectory planning. More...

bool	isManeuverPlanValid (const carma_planning_msgs::msg::ManeuverPlan &maneuver_plan) const noexcept
	Example if a maneuver plan contains at least one maneuver. More...

bool	isTrajectoryValid (const carma_planning_msgs::msg::TrajectoryPlan &trajectory_plan) const noexcept
	Example if a trajectory plan contains at least two trajectory points. More...

bool	isTrajectoryLongEnough (const carma_planning_msgs::msg::TrajectoryPlan &plan) const noexcept
	Example if a trajectory plan is longer than configured time thresheld. More...

carma_planning_msgs::msg::TrajectoryPlan	planTrajectory ()
	Plan trajectory based on latest maneuver plan via ROS service call to plugins. More...

LaneChangeInformation	getLaneChangeInformation (const carma_planning_msgs::msg::Maneuver &lane_change_maneuver)
	Function for generating a LaneChangeInformation object from a provided lane change maneuver. More...

void	publishUpcomingLaneChangeStatus (const boost::optional< LaneChangeInformation > &upcoming_lane_change_information)
	Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty, an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published UpcomingLaneChangeStatus message is stored in upcoming_lane_change_status_. More...

void	publishTurnSignalCommand (const boost::optional< LaneChangeInformation > &current_lane_change_information, const carma_planning_msgs::msg::UpcomingLaneChangeStatus &upcoming_lane_change_status)
	Function for processing an optional LaneChangeInformation object pertaining to the currently-occurring lane change and an UpcomingLaneChangeStatus message. If the optional object pertaining to the currently-occurring lane change is not empty, then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction of the UpcomingLaneChangeStatus message is published if the vehicle is estimated to begin that lane change in under the time threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in latest_turn_signal_command_. More...

	FRIEND_TEST (TestPlanDelegator, UnitTestPlanDelegator)

	FRIEND_TEST (TestPlanDelegator, TestPlanDelegator)

	FRIEND_TEST (TestPlanDelegator, TestLaneChangeInformation)

	FRIEND_TEST (TestPlanDelegator, TestUpcomingLaneChangeAndTurnSignals)

	FRIEND_TEST (TestPlanDelegator, TestUpdateManeuverParameters)

Private Attributes
std::optional< carma_planning_msgs::msg::TrajectoryPlan >	last_successful_traj_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::TrajectoryPlan >	traj_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::UpcomingLaneChangeStatus >	upcoming_lane_change_status_pub_

carma_ros2_utils::PubPtr< autoware_msgs::msg::LampCmd >	turn_signal_command_pub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::ManeuverPlan >	plan_sub_

carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped >	pose_sub_

carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped >	twist_sub_

carma_ros2_utils::SubPtr< carma_planning_msgs::msg::GuidanceState >	guidance_state_sub_

rclcpp::TimerBase::SharedPtr	traj_timer_

rclcpp::CallbackGroup::SharedPtr	timer_callback_group_

bool	guidance_engaged = false

int	consecutive_traj_gen_failure_num_ = 0

double	length_to_front_bumper_ = 3.0

std::shared_ptr< tf2_ros::Buffer >	tf2_buffer_

std::shared_ptr< tf2_ros::TransformListener >	tf2_listener_

boost::optional< LaneChangeInformation >	upcoming_lane_change_information_

boost::optional< LaneChangeInformation >	current_lane_change_information_

carma_planning_msgs::msg::UpcomingLaneChangeStatus	upcoming_lane_change_status_

autoware_msgs::msg::LampCmd	latest_turn_signal_command_

Detailed Description

Definition at line 110 of file plan_delegator.hpp.

Constructor & Destructor Documentation

◆ PlanDelegator()

plan_delegator::PlanDelegator::PlanDelegator ( const rclcpp::NodeOptions & options )

explicit

PlanDelegator constructor.

Definition at line 131 of file plan_delegator.cpp.

                                                                 : carma_ros2_utils::CarmaLifecycleNode(options),
                                                                        tf2_buffer_(std::make_shared<tf2_ros::Buffer>(this->get_clock())),
                                                                        wml_(this->get_node_base_interface(), this->get_node_logging_interface(),
                                                                            this->get_node_topics_interface(), this->get_node_parameters_interface())
    {
        // Create initial config
        config_ = Config();
 
        config_.planning_topic_prefix = declare_parameter<std::string>("planning_topic_prefix", config_.planning_topic_prefix);
        config_.planning_topic_suffix = declare_parameter<std::string>("planning_topic_suffix", config_.planning_topic_suffix);
        config_.trajectory_planning_rate = declare_parameter<double>("trajectory_planning_rate", config_.trajectory_planning_rate);
        config_.max_trajectory_duration = declare_parameter<double>("trajectory_duration_threshold", config_.max_trajectory_duration);
        config_.min_crawl_speed = declare_parameter<double>("min_speed", config_.min_crawl_speed);
        config_.duration_to_signal_before_lane_change = declare_parameter<double>("duration_to_signal_before_lane_change", config_.duration_to_signal_before_lane_change);
        config_.max_traj_generation_reattempt = declare_parameter<int>("max_traj_generation_reattempt", config_.max_traj_generation_reattempt);
        config_.tactical_plugin_service_call_timeout = declare_parameter<int>("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout);
    }

References config_, plan_delegator::Config::duration_to_signal_before_lane_change, plan_delegator::Config::max_traj_generation_reattempt, plan_delegator::Config::max_trajectory_duration, plan_delegator::Config::min_crawl_speed, plan_delegator::Config::planning_topic_prefix, plan_delegator::Config::planning_topic_suffix, plan_delegator::Config::tactical_plugin_service_call_timeout, and plan_delegator::Config::trajectory_planning_rate.

Member Function Documentation

◆ composePlanTrajectoryRequest()

std::shared_ptr< carma_planning_msgs::srv::PlanTrajectory::Request > plan_delegator::PlanDelegator::composePlanTrajectoryRequest	(	const carma_planning_msgs::msg::TrajectoryPlan &	latest_trajectory_plan,
		const carma_planning_msgs::msg::ManeuverPlan &	locked_maneuver_plan,
		const uint16_t &	current_maneuver_index
	)		const

Generate new PlanTrajecory service request based on current planning progress.

Parameters

latest_trajectory_plan	The trajectory plan to append the resulting trajectory
locked_maneuver_plan	The maneuver plan to send to the tactical plugin
current_maneuver_index	The idx of the maneuver in the maneuver plan that this request is for

Returns: a PlanTrajectoryRequest to be used in the service call to the tactical plugin

Definition at line 418 of file plan_delegator.cpp.

    {
        auto plan_req = std::make_shared<carma_planning_msgs::srv::PlanTrajectory::Request>();
        plan_req->maneuver_plan = locked_maneuver_plan;
 
        // set current vehicle state if we have NOT planned any previous trajectories
        if(latest_trajectory_plan.trajectory_points.empty())
        {
            plan_req->header.stamp = latest_pose_.header.stamp;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "latest_pose_.header.stamp: " << std::to_string(rclcpp::Time(latest_pose_.header.stamp).seconds()));
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "plan_req->header.stamp: " << std::to_string(rclcpp::Time(plan_req->header.stamp).seconds()));
 
            plan_req->vehicle_state.longitudinal_vel = latest_twist_.twist.linear.x;
            plan_req->vehicle_state.x_pos_global = latest_pose_.pose.position.x;
            plan_req->vehicle_state.y_pos_global = latest_pose_.pose.position.y;
            double roll, pitch, yaw;
            carma_wm::geometry::rpyFromQuaternion(latest_pose_.pose.orientation, roll, pitch, yaw);
            plan_req->vehicle_state.orientation = yaw;
            plan_req->maneuver_index_to_plan = current_maneuver_index;
        }
        // set vehicle state based on last two planned trajectory points
        else
        {
            carma_planning_msgs::msg::TrajectoryPlanPoint last_point = latest_trajectory_plan.trajectory_points.back();
            carma_planning_msgs::msg::TrajectoryPlanPoint second_last_point = *(latest_trajectory_plan.trajectory_points.rbegin() + 1);
            plan_req->vehicle_state.x_pos_global = last_point.x;
            plan_req->vehicle_state.y_pos_global = last_point.y;
            auto distance_diff = std::sqrt(std::pow(last_point.x - second_last_point.x, 2) + std::pow(last_point.y - second_last_point.y, 2));
            rclcpp::Duration time_diff = rclcpp::Time(last_point.target_time) - rclcpp::Time(second_last_point.target_time);
            auto time_diff_sec = time_diff.seconds();
            plan_req->maneuver_index_to_plan = current_maneuver_index;
            // this assumes the vehicle does not have significant lateral velocity
            plan_req->header.stamp = latest_trajectory_plan.trajectory_points.back().target_time;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "plan_req->header.stamp: " << std::to_string(rclcpp::Time(plan_req->header.stamp).seconds()));
 
            plan_req->vehicle_state.longitudinal_vel = distance_diff / time_diff_sec;
            // TODO develop way to set yaw value for future points
        }
        return plan_req;
    }

References latest_pose_, latest_twist_, carma_wm::geometry::rpyFromQuaternion(), and carma_cooperative_perception::to_string().

Referenced by planTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ FRIEND_TEST() [1/5]

plan_delegator::PlanDelegator::FRIEND_TEST	(	TestPlanDelegator	,
		TestLaneChangeInformation
	)

private

◆ FRIEND_TEST() [2/5]

plan_delegator::PlanDelegator::FRIEND_TEST	(	TestPlanDelegator	,
		TestPlanDelegator
	)

private

◆ FRIEND_TEST() [3/5]

plan_delegator::PlanDelegator::FRIEND_TEST	(	TestPlanDelegator	,
		TestUpcomingLaneChangeAndTurnSignals
	)

private

◆ FRIEND_TEST() [4/5]

plan_delegator::PlanDelegator::FRIEND_TEST	(	TestPlanDelegator	,
		TestUpdateManeuverParameters
	)

private

◆ FRIEND_TEST() [5/5]

plan_delegator::PlanDelegator::FRIEND_TEST	(	TestPlanDelegator	,
		UnitTestPlanDelegator
	)

private

◆ getLaneChangeInformation()

LaneChangeInformation plan_delegator::PlanDelegator::getLaneChangeInformation ( const carma_planning_msgs::msg::Maneuver & lane_change_maneuver )

private

Function for generating a LaneChangeInformation object from a provided lane change maneuver.

Parameters

lane_change_maneuver The lane change maneuver that a LaneChangeInformation object shall be generated from.

Returns: A LaneChangeInformation object containing information on the provided lane change maneuver.

Definition at line 258 of file plan_delegator.cpp.

                                                                                                                           {
        LaneChangeInformation lane_change_information;
 
        lane_change_information.starting_downtrack = lane_change_maneuver.lane_change_maneuver.start_dist;
 
        // Get the starting and ending lanelets for this lane change maneuver
        lanelet::ConstLanelet starting_lanelet = wm_->getMap()->laneletLayer.get(std::stoi(lane_change_maneuver.lane_change_maneuver.starting_lane_id));
        lanelet::ConstLanelet ending_lanelet = wm_->getMap()->laneletLayer.get(std::stoi(lane_change_maneuver.lane_change_maneuver.ending_lane_id));
 
        // Determine if lane change is a left or right lane change and update lane_change_information accordingly
        bool shared_boundary_found = false;
 
        lanelet::ConstLanelet current_lanelet = starting_lanelet;
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Searching for shared boundary with starting lanechange lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id()));
        while(!shared_boundary_found){
            // Assumption: Adjacent lanelets share lane boundary
 
            if(current_lanelet.leftBound() == ending_lanelet.rightBound()){
                // If current lanelet's left lane boundary matches the ending lanelet's right lane boundary, it is a left lane change
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id()));
                lane_change_information.is_right_lane_change = false;
                shared_boundary_found = true;
            }
            else if(current_lanelet.rightBound() == ending_lanelet.leftBound()){
                // If current lanelet's right lane boundary matches the ending lanelet's left lane boundary, it is a right lane change
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Lanelet " << std::to_string(current_lanelet.id()) << " shares right boundary with " << std::to_string(ending_lanelet.id()));
                lane_change_information.is_right_lane_change = true;
                shared_boundary_found = true;
            }
            else{
                // If there are no following lanelets on route, lanechange should be completing before reaching it
                if(wm_->getMapRoutingGraph()->following(current_lanelet, false).empty())
                {
                    // Maneuver requires we travel further before completing lane change, but there is no routable lanelet directly ahead;
                    // in this case we have reached a lanelet which does not have a routable lanelet ahead and isn't adjacent to the lanelet where lane change ends.
                    // A lane change should have already happened at this point
                    throw(std::invalid_argument("No following lanelets from current lanelet reachable without a lane change, incorrectly chosen end lanelet"));
                }
 
                current_lanelet = wm_->getMapRoutingGraph()->following(current_lanelet, false).front();
                if(current_lanelet.id() == starting_lanelet.id()){
                    //Looped back to starting lanelet
                    throw(std::invalid_argument("No lane change in path"));
                }
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Now checking for shared lane boundary with lanelet " << std::to_string(current_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id()));
            }
        }
 
        return lane_change_information;
    }

References plan_delegator::LaneChangeInformation::is_right_lane_change, plan_delegator::LaneChangeInformation::starting_downtrack, carma_cooperative_perception::to_string(), and wm_.

Referenced by maneuverPlanCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getPlannerClientByName()

carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory > plan_delegator::PlanDelegator::getPlannerClientByName ( const std::string & planner_name )

Get PlanTrajectory service client by plugin name and create new PlanTrajectory service client if specified name does not exist.

Returns: a ServiceClient object which corresponse to the target planner

Definition at line 378 of file plan_delegator.cpp.

    {
        if(planner_name.size() == 0)
        {
            throw std::invalid_argument("Invalid trajectory planner name because it has zero length!");
        }
        if(trajectory_planners_.find(planner_name) == trajectory_planners_.end())
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Discovered new trajectory planner: " << planner_name);
 
            trajectory_planners_.emplace(
                planner_name, create_client<carma_planning_msgs::srv::PlanTrajectory>(config_.planning_topic_prefix + planner_name + config_.planning_topic_suffix));
        }
        return trajectory_planners_[planner_name];
    }

References config_, plan_delegator::Config::planning_topic_prefix, plan_delegator::Config::planning_topic_suffix, and trajectory_planners_.

Referenced by planTrajectory().

Here is the caller graph for this function:

◆ guidanceStateCallback()

void plan_delegator::PlanDelegator::guidanceStateCallback ( carma_planning_msgs::msg::GuidanceState::UniquePtr plan )

Callback function of guidance state subscriber.

Definition at line 191 of file plan_delegator.cpp.

    {
        guidance_engaged = (msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED);
    }

References lightbar_manager::ENGAGED, and guidance_engaged.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ handle_on_activate()

carma_ros2_utils::CallbackReturn plan_delegator::PlanDelegator::handle_on_activate ( const rclcpp_lifecycle::State & )

Definition at line 182 of file plan_delegator.cpp.

    {
        timer_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        traj_timer_ = create_timer(get_clock(),
            std::chrono::milliseconds((int)(1 / config_.trajectory_planning_rate * 1000)),
            std::bind(&PlanDelegator::onTrajPlanTick, this), timer_callback_group_);
         return CallbackReturn::SUCCESS;
    }

References config_, onTrajPlanTick(), timer_callback_group_, traj_timer_, and plan_delegator::Config::trajectory_planning_rate.

Here is the call graph for this function:

◆ handle_on_configure()

carma_ros2_utils::CallbackReturn plan_delegator::PlanDelegator::handle_on_configure ( const rclcpp_lifecycle::State & )

Definition at line 149 of file plan_delegator.cpp.

    {
        // Reset config
        config_ = Config();
 
        get_parameter<std::string>("planning_topic_prefix", config_.planning_topic_prefix);
        get_parameter<std::string>("planning_topic_suffix", config_.planning_topic_suffix);
        get_parameter<double>("trajectory_planning_rate", config_.trajectory_planning_rate);
        get_parameter<double>("trajectory_duration_threshold", config_.max_trajectory_duration);
        get_parameter<double>("min_speed", config_.min_crawl_speed);
        get_parameter<double>("duration_to_signal_before_lane_change", config_.duration_to_signal_before_lane_change);
        get_parameter<int>("tactical_plugin_service_call_timeout", config_.tactical_plugin_service_call_timeout);
        get_parameter<int>("max_traj_generation_reattempt", config_.max_traj_generation_reattempt);
 
        RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Done loading parameters: " << config_);
 
        // Setup publishers
        traj_pub_ = create_publisher<carma_planning_msgs::msg::TrajectoryPlan>("plan_trajectory", 5);
        upcoming_lane_change_status_pub_ = create_publisher<carma_planning_msgs::msg::UpcomingLaneChangeStatus>("upcoming_lane_change_status", 1);
        turn_signal_command_pub_ = create_publisher<autoware_msgs::msg::LampCmd>("lamp_cmd", 1);
 
        // Setup subscribers
        plan_sub_ = create_subscription<carma_planning_msgs::msg::ManeuverPlan>("final_maneuver_plan", 5, std::bind(&PlanDelegator::maneuverPlanCallback, this, std_ph::_1));
        twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 5,
            [this](geometry_msgs::msg::TwistStamped::UniquePtr twist) {this->latest_twist_ = *twist;});
        pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 5, std::bind(&PlanDelegator::poseCallback, this, std_ph::_1));
        guidance_state_sub_ = create_subscription<carma_planning_msgs::msg::GuidanceState>("guidance_state", 5,  std::bind(&PlanDelegator::guidanceStateCallback, this, std_ph::_1));
 
        lookupFrontBumperTransform();
        wm_ = wml_.getWorldModel();
        return CallbackReturn::SUCCESS;
    }

Here is the call graph for this function:

◆ isManeuverExpired()

bool plan_delegator::PlanDelegator::isManeuverExpired	(	const carma_planning_msgs::msg::Maneuver &	maneuver,
		rclcpp::Time	current_time
	)		const

Example if a maneuver end time has passed current system time.

Returns: if input maneuver is expires NOTE: current_time is assumed to be same clock type as this node

Definition at line 406 of file plan_delegator.cpp.

    {
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "maneuver start time:" << std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, start_time)).seconds()));
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "maneuver end time:" << std::to_string(rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time)).seconds()));
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "current time:" << std::to_string(now().seconds()));
        bool isexpired = rclcpp::Time(GET_MANEUVER_PROPERTY(maneuver, end_time), get_clock()->get_clock_type()) <= current_time; // TODO maneuver expiration should maybe be based off of distance not time? https://github.com/usdot-fhwa-stol/carma-platform/issues/1107
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "isexpired:" << isexpired);
        // TODO: temporary disabling expiration check
        return false;
    }

References GET_MANEUVER_PROPERTY, and carma_cooperative_perception::to_string().

Referenced by planTrajectory().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isManeuverPlanValid()

bool plan_delegator::PlanDelegator::isManeuverPlanValid ( const carma_planning_msgs::msg::ManeuverPlan & maneuver_plan ) const

privatenoexcept

Example if a maneuver plan contains at least one maneuver.

Returns: if input maneuver plan is valid

Definition at line 394 of file plan_delegator.cpp.

    {
        // currently it only checks if maneuver list is empty
        return !maneuver_plan.maneuvers.empty();
    }

Referenced by maneuverPlanCallback().

Here is the caller graph for this function:

◆ isTrajectoryLongEnough()

bool plan_delegator::PlanDelegator::isTrajectoryLongEnough ( const carma_planning_msgs::msg::TrajectoryPlan & plan ) const

privatenoexcept

Example if a trajectory plan is longer than configured time thresheld.

Returns: if input trajectory plan is long enough

Definition at line 462 of file plan_delegator.cpp.

    {
        rclcpp::Duration time_diff = rclcpp::Time(plan.trajectory_points.back().target_time) - rclcpp::Time(plan.trajectory_points.front().target_time);
        return time_diff.seconds() >= config_.max_trajectory_duration;
    }

Referenced by planTrajectory().

Here is the caller graph for this function:

◆ isTrajectoryValid()

bool plan_delegator::PlanDelegator::isTrajectoryValid ( const carma_planning_msgs::msg::TrajectoryPlan & trajectory_plan ) const

privatenoexcept

Example if a trajectory plan contains at least two trajectory points.

Returns: if input trajectory plan is valid

Definition at line 400 of file plan_delegator.cpp.

    {
        // currently it only checks if trajectory contains less than 2 points
        return !(trajectory_plan.trajectory_points.size() < 2);
    }

Referenced by onTrajPlanTick(), and planTrajectory().

Here is the caller graph for this function:

◆ lookupFrontBumperTransform()

void plan_delegator::PlanDelegator::lookupFrontBumperTransform ( )

Lookup transfrom from front bumper to base link.

Definition at line 780 of file plan_delegator.cpp.

    {
        tf2_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer_);
        tf2_buffer_->setUsingDedicatedThread(true);
        try
        {
            geometry_msgs::msg::TransformStamped tf = tf2_buffer_->lookupTransform("base_link", "vehicle_front", rclcpp::Time(0), rclcpp::Duration(20.0, 0)); //save to local copy of transform 20 sec timeout
            length_to_front_bumper_ = tf.transform.translation.x;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"length_to_front_bumper_: " << length_to_front_bumper_);
 
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), ex.what());
        }
    }

References length_to_front_bumper_, tf2_buffer_, and tf2_listener_.

Referenced by handle_on_configure().

Here is the caller graph for this function:

◆ maneuverPlanCallback()

void plan_delegator::PlanDelegator::maneuverPlanCallback ( carma_planning_msgs::msg::ManeuverPlan::UniquePtr plan )

Callback function of maneuver plan subscriber.

Definition at line 196 of file plan_delegator.cpp.

    {
        RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Received request to delegate plan ID " << std::string(plan->maneuver_plan_id));
        // do basic check to see if the input is valid
        auto copy_plan = *plan;
        received_maneuver_plan_ = true;
        if (isManeuverPlanValid(copy_plan))
        {
            latest_maneuver_plan_ = copy_plan;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Received plan with " << latest_maneuver_plan_.maneuvers.size() << " maneuvers");
 
            // Update the parameters associated with each maneuver
            for (auto& maneuver : latest_maneuver_plan_.maneuvers) {
                updateManeuverParameters(maneuver);
            }
        }
        else {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),"Received empty plan, no maneuvers found in plan ID " << std::string(plan->maneuver_plan_id));
        }
 
        // Update upcoming_lane_change_information_ and current_lane_change_information_ based on the received maneuver plan
        if(!latest_maneuver_plan_.maneuvers.empty()){
            // Get ego vehicle's current downtrack
            lanelet::BasicPoint2d current_loc(latest_pose_.pose.position.x, latest_pose_.pose.position.y);
            double current_downtrack = wm_->routeTrackPos(current_loc).downtrack;
 
            // Set upcoming_lane_change_information_ based on the first found lane change in the plan that begins after current_downtrack, if one exists
            upcoming_lane_change_information_ = boost::optional<LaneChangeInformation>(); // Reset to empty optional
            for(const auto& maneuver : latest_maneuver_plan_.maneuvers){
                if(maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE){
                    if(current_downtrack >= maneuver.lane_change_maneuver.start_dist){
                        // Skip this lane change maneuver since ego vehicle has passed the lane change start point (this is not an 'upcoming' lane change)
                        continue;
                    }
                    else{
                        LaneChangeInformation upcoming_lane_change_information = getLaneChangeInformation(maneuver);
                        upcoming_lane_change_information_ = boost::optional<LaneChangeInformation>(upcoming_lane_change_information);
                        break;
                    }
                }
            }
 
            // Set current_lane_change_information_ if the first maneuver is a lane change
            current_lane_change_information_ = boost::optional<LaneChangeInformation>(); // Reset to empty optional
            if(latest_maneuver_plan_.maneuvers[0].type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE){
                LaneChangeInformation current_lane_change_information = getLaneChangeInformation(latest_maneuver_plan_.maneuvers[0]);
                current_lane_change_information_ = boost::optional<LaneChangeInformation>(current_lane_change_information);
            }
        }
    }

References current_lane_change_information_, getLaneChangeInformation(), isManeuverPlanValid(), latest_maneuver_plan_, latest_pose_, received_maneuver_plan_, upcoming_lane_change_information_, updateManeuverParameters(), and wm_.

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onTrajPlanTick()

void plan_delegator::PlanDelegator::onTrajPlanTick ( )

private

Callback function for triggering trajectory planning.

Definition at line 726 of file plan_delegator.cpp.

    {
        // Guidance not engaged or haven't received a maneuver plan yet
        if (!guidance_engaged || !received_maneuver_plan_)
        {
            return;
        }
        carma_planning_msgs::msg::TrajectoryPlan trajectory_plan = planTrajectory();
 
        // Check if planned trajectory is valid before send out
        if(isTrajectoryValid(trajectory_plan))
        {
            trajectory_plan.header.stamp = get_clock()->now();
            last_successful_traj_ = trajectory_plan;
            traj_pub_->publish(trajectory_plan);
            consecutive_traj_gen_failure_num_ = 0;
        }
        else
        {
            consecutive_traj_gen_failure_num_ ++;
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),
                "Guidance is engaged, but new planned trajectory has less than 2 points. " <<
                "It will not be published! Consecutive failure count: "
                << consecutive_traj_gen_failure_num_);
 
            // Case where traj generation fails after a successful one
            if (last_successful_traj_.has_value()
                && consecutive_traj_gen_failure_num_
                    <= config_.max_traj_generation_reattempt)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),
                    "Instead, last available trajectory is published with outdated timestamp of:"
                    << std::to_string(
                        rclcpp::Time(last_successful_traj_.value().header.stamp).seconds()));
                traj_pub_->publish(last_successful_traj_.value());
            }
            // Case where traj generation fails from the beginning.
            // Attempt replanning for configured number of tries before throwing runtime error.
            else if (!last_successful_traj_.has_value() &&
                consecutive_traj_gen_failure_num_ <= config_.max_traj_generation_reattempt)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),
                    "Instead, tried publishing last available trajectory, but it's not available!");
            }
            else
            {
                RCLCPP_ERROR_STREAM(rclcpp::get_logger("plan_delegator"),
                    "No valid trajectory is available to publish! "
                    "Please check the planner plugins and their configurations.");
                throw std::runtime_error("No valid trajectory is available to publish!");
            }
        }
    }

References config_, consecutive_traj_gen_failure_num_, guidance_engaged, isTrajectoryValid(), last_successful_traj_, plan_delegator::Config::max_traj_generation_reattempt, planTrajectory(), received_maneuver_plan_, carma_cooperative_perception::to_string(), and traj_pub_.

Referenced by handle_on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ planTrajectory()

carma_planning_msgs::msg::TrajectoryPlan plan_delegator::PlanDelegator::planTrajectory ( )

private

Plan trajectory based on latest maneuver plan via ROS service call to plugins.

Returns: a TrajectoryPlan object which contains PlanTrajectory response from plugins

Definition at line 599 of file plan_delegator.cpp.

    {
        carma_planning_msgs::msg::TrajectoryPlan latest_trajectory_plan;
        bool full_plan_generation_failed = false;
        if(!guidance_engaged)
        {
            RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Guidance is not engaged. Plan delegator will not plan trajectory.");
            return latest_trajectory_plan;
        }
        // latest_maneuver_plan may get updated, so local copy to avoid race condition
        auto locked_maneuver_plan = latest_maneuver_plan_;
 
        // Flag for the first received trajectory plan service response
        bool first_trajectory_plan = true;
 
        // Track the index of the starting maneuver in the maneuver plan that this trajectory plan service request is for
        uint16_t current_maneuver_index = 0;
 
        // Loop through maneuver list to make service call to applicable Tactical Plugin
        while(current_maneuver_index < locked_maneuver_plan.maneuvers.size())
        {
            auto& maneuver = locked_maneuver_plan.maneuvers[current_maneuver_index];
 
            // ignore expired maneuvers
            if(isManeuverExpired(maneuver, get_clock()->now()))
            {
                RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Dropping expired maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id));
                // Update the maneuver plan index for the next loop
                ++current_maneuver_index;
                continue;
            }
            lanelet::BasicPoint2d current_loc(latest_pose_.pose.position.x, latest_pose_.pose.position.y);
            double current_downtrack = wm_->routeTrackPos(current_loc).downtrack;
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"current_downtrack" << current_downtrack);
            double maneuver_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"maneuver_end_dist" << maneuver_end_dist);
 
            // ignore maneuver that is passed.
            if (current_downtrack > maneuver_end_dist)
            {
                RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Dropping passed maneuver: " << GET_MANEUVER_PROPERTY(maneuver, parameters.maneuver_id));
                // Update the maneuver plan index for the next loop
                ++current_maneuver_index;
                continue;
            }
 
            // get corresponding ros service client for plan trajectory
            auto maneuver_planner = GET_MANEUVER_PROPERTY(maneuver, parameters.planning_tactical_plugin);
 
            auto client = getPlannerClientByName(maneuver_planner);
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Current planner: " << maneuver_planner);
 
            // compose service request
            auto plan_req = composePlanTrajectoryRequest(
                latest_trajectory_plan, locked_maneuver_plan, current_maneuver_index);
 
            auto future_response = client->async_send_request(plan_req);
 
            auto future_status = future_response.wait_for(std::chrono::milliseconds(config_.tactical_plugin_service_call_timeout));
 
            if (future_status != std::future_status::ready)
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),"Unsuccessful service call to trajectory planner:" << maneuver_planner << " for plan ID " << std::string(locked_maneuver_plan.maneuver_plan_id));
                // if one service call fails, it should end plan immediately because it is there is no point to generate plan with empty space
                full_plan_generation_failed = true;
                break;
            }
 
            // If successful service request
            auto plan_response = future_response.get();
            // validate trajectory before add to the plan
            if(!isTrajectoryValid(plan_response->trajectory_plan))
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),
                    "Found invalid trajectory with less than 2 trajectory "
                    << "points for maneuver_plan_id: "
                    << std::string(locked_maneuver_plan.maneuver_plan_id));
                full_plan_generation_failed = true;
                break;
            }
            //Remove duplicate point from start of trajectory
            if(latest_trajectory_plan.trajectory_points.size() != 0 &&
                latest_trajectory_plan.trajectory_points.back().target_time == plan_response->trajectory_plan.trajectory_points.front().target_time)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Removing duplicate point for planner: " << maneuver_planner);
                plan_response->trajectory_plan.trajectory_points.erase(plan_response->trajectory_plan.trajectory_points.begin());
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"plan_response->trajectory_plan size: " << plan_response->trajectory_plan.trajectory_points.size());
            }
            latest_trajectory_plan.trajectory_points.insert(latest_trajectory_plan.trajectory_points.end(),
                                                            plan_response->trajectory_plan.trajectory_points.begin(),
                                                            plan_response->trajectory_plan.trajectory_points.end());
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"new latest_trajectory_plan size: " << latest_trajectory_plan.trajectory_points.size());
 
            // Assign the trajectory plan's initial longitudinal velocity based on the first tactical plugin's response
            if(first_trajectory_plan == true)
            {
                latest_trajectory_plan.initial_longitudinal_velocity = plan_response->trajectory_plan.initial_longitudinal_velocity;
                first_trajectory_plan = false;
            }
 
            if(isTrajectoryLongEnough(latest_trajectory_plan))
            {
                RCLCPP_INFO_STREAM(rclcpp::get_logger("plan_delegator"),"Plan Trajectory completed for " << std::string(locked_maneuver_plan.maneuver_plan_id));
                break;
            }
 
            // Update the maneuver plan index based on the last maneuver index converted to a trajectory
            // This is required since inlanecruising_plugin can plan a trajectory over contiguous LANE_FOLLOWING maneuvers
            if(plan_response->related_maneuvers.size() > 0)
            {
                current_maneuver_index = plan_response->related_maneuvers.back() + 1;
            }
        }
 
        if (full_plan_generation_failed)
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),
                "Plan_delegator's current run wasn't fully able to generate trajectory!");
 
            carma_planning_msgs::msg::TrajectoryPlan empty_plan;
            return empty_plan;
        }
 
        return latest_trajectory_plan;
    }

References composePlanTrajectoryRequest(), config_, GET_MANEUVER_PROPERTY, getPlannerClientByName(), guidance_engaged, isManeuverExpired(), isTrajectoryLongEnough(), isTrajectoryValid(), latest_maneuver_plan_, latest_pose_, plan_delegator::Config::tactical_plugin_service_call_timeout, and wm_.

Referenced by onTrajPlanTick().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ poseCallback()

void plan_delegator::PlanDelegator::poseCallback ( geometry_msgs::msg::PoseStamped::UniquePtr pose_msg )

Callback function for vehicle pose subscriber. Updates latest_pose_ and makes calls to publishUpcomingLaneChangeStatus() and publishTurnSignalCommand().

Parameters

pose_msg The received pose message.

Definition at line 247 of file plan_delegator.cpp.

    {
        latest_pose_ = *pose_msg;
 
        // Publish the upcoming lane change status
        publishUpcomingLaneChangeStatus(upcoming_lane_change_information_);
 
        // Publish the current turn signal command
        publishTurnSignalCommand(current_lane_change_information_, upcoming_lane_change_status_);
    }

References current_lane_change_information_, latest_pose_, publishTurnSignalCommand(), publishUpcomingLaneChangeStatus(), upcoming_lane_change_information_, and upcoming_lane_change_status_.

Referenced by handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ publishTurnSignalCommand()

void plan_delegator::PlanDelegator::publishTurnSignalCommand	(	const boost::optional< LaneChangeInformation > &	current_lane_change_information,
		const carma_planning_msgs::msg::UpcomingLaneChangeStatus &	upcoming_lane_change_status
	)

private

Function for processing an optional LaneChangeInformation object pertaining to the currently-occurring lane change and an UpcomingLaneChangeStatus message. If the optional object pertaining to the currently-occurring lane change is not empty, then a turn signal command is published based on the current lane change direction. Otherwise, a turn signal command in the direction of the UpcomingLaneChangeStatus message is published if the vehicle is estimated to begin that lane change in under the time threshold defined by config_.duration_to_signal_before_lane_change. The published TurnSignalComand message is stored in latest_turn_signal_command_.

Parameters

current_lane_change_information	An optional LaneChangeInformation object pertaining to the current lane change. Empty if vehicle is not currently changing lanes.
upcoming_lane_change_status	An UpcomingLaneChangeStatus message containing the lane change direction of an upcoming lane change, along with the downtrack distance to that lane change.

Definition at line 340 of file plan_delegator.cpp.

    {
        // Initialize turn signal command message
        // NOTE: A LampCmd message can have its 'r' OR 'l' field set to 1 to indicate an activated right or left turn signal, respectively. Both fields cannot be set to 1 at the same time.
        autoware_msgs::msg::LampCmd turn_signal_command;
 
        // Publish turn signal command with priority placed on the current lane change, if one exists
        if(current_lane_change_information){
            // Publish turn signal command for the current lane change based on the lane change direction
            if(current_lane_change_information.get().is_right_lane_change){
                turn_signal_command.r = 1;
            }
            else{
                turn_signal_command.l = 1;
            }
            turn_signal_command_pub_->publish(turn_signal_command);
        }
        else if(upcoming_lane_change_status.lane_change != carma_planning_msgs::msg::UpcomingLaneChangeStatus::NONE){
            // Only publish turn signal command for upcoming lane change if it will begin in less than the time defined by config_.duration_to_signal_before_lane_change
            if((upcoming_lane_change_status.downtrack_until_lanechange / latest_twist_.twist.linear.x) <= config_.duration_to_signal_before_lane_change){
                if(upcoming_lane_change_status.lane_change == carma_planning_msgs::msg::UpcomingLaneChangeStatus::RIGHT){
                    turn_signal_command.r = 1;
                }
                else{
                    turn_signal_command.l = 1;
                }
                turn_signal_command_pub_->publish(turn_signal_command);
            }
        }
        else{
            // Publish turn signal command with neither turn signal activated
            turn_signal_command_pub_->publish(turn_signal_command);
        }
 
        // Store turn signal command in latest_turn_signal_command_
        latest_turn_signal_command_ = turn_signal_command;
    }

References config_, plan_delegator::Config::duration_to_signal_before_lane_change, latest_turn_signal_command_, latest_twist_, and turn_signal_command_pub_.

Referenced by poseCallback().

Here is the caller graph for this function:

◆ publishUpcomingLaneChangeStatus()

void plan_delegator::PlanDelegator::publishUpcomingLaneChangeStatus ( const boost::optional< LaneChangeInformation > & upcoming_lane_change_information )

private

Function for processing an optional LaneChangeInformation object pertaining to an upcoming lane change. If not empty, an UpcomingLaneChangeStatus message is created and published based on the contents of the LaneChangeInformation. The published UpcomingLaneChangeStatus message is stored in upcoming_lane_change_status_.

Parameters

upcoming_lane_change_information An optional LaneChangeInformation object. Empty if no upcoming lane change exists.

Definition at line 310 of file plan_delegator.cpp.

                                                                                                                                   {
        // Initialize an UpcomingLaneChangeStatus message, which will be populated based on upcoming_lane_change_information
        carma_planning_msgs::msg::UpcomingLaneChangeStatus upcoming_lane_change_status;
 
        // Update upcoming_lane_change_status
        if(upcoming_lane_change_information){
            // Get the downtrack distance between the ego vehicle and the start of the upcoming lane change maneuver
            lanelet::BasicPoint2d current_loc(latest_pose_.pose.position.x, latest_pose_.pose.position.y);
            double current_downtrack = wm_->routeTrackPos(current_loc).downtrack;
            upcoming_lane_change_status.downtrack_until_lanechange = std::max(0.0, upcoming_lane_change_information.get().starting_downtrack - current_downtrack);
 
            // Set upcoming lane change status as a right lane change or left lane change
            if(upcoming_lane_change_information.get().is_right_lane_change){
                upcoming_lane_change_status.lane_change = carma_planning_msgs::msg::UpcomingLaneChangeStatus::RIGHT;
            }
            else{
                upcoming_lane_change_status.lane_change = carma_planning_msgs::msg::UpcomingLaneChangeStatus::LEFT;
            }
        }
        else{
            upcoming_lane_change_status.lane_change = carma_planning_msgs::msg::UpcomingLaneChangeStatus::NONE;
        }
 
        // Publish upcoming_lane_change_status
        upcoming_lane_change_status_pub_->publish(upcoming_lane_change_status);
 
        // Store UpcomingLaneChangeStatus in upcoming_lane_change_status_
        upcoming_lane_change_status_ = upcoming_lane_change_status;
    }

References latest_pose_, upcoming_lane_change_status_, upcoming_lane_change_status_pub_, and wm_.

Referenced by poseCallback().

Here is the caller graph for this function:

◆ updateManeuverParameters()

void plan_delegator::PlanDelegator::updateManeuverParameters ( carma_planning_msgs::msg::Maneuver & maneuver )

Update the starting downtrack, ending downtrack, and maneuver-specific Lanelet ID parameters associated with a given maneuver. These updates are required since the starting and ending downtrack values of each maneuver are shifted based on the distance between the base_link frame and the vehicle_front frame.

Parameters

maneuver The maneuver to be updated.

Definition at line 468 of file plan_delegator.cpp.

    {
        if (!wm_->getMap())
        {
            RCLCPP_ERROR_STREAM(rclcpp::get_logger("plan_delegator"), "Map is not set yet");
            return;
        }
 
        // Update maneuver starting and ending downtrack distances
        double original_start_dist = GET_MANEUVER_PROPERTY(maneuver, start_dist);
        double original_end_dist = GET_MANEUVER_PROPERTY(maneuver, end_dist);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Changing maneuver distances for planner: " << GET_MANEUVER_PROPERTY(maneuver, parameters.planning_tactical_plugin));
        double adjusted_start_dist = original_start_dist - length_to_front_bumper_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"original_start_dist:" << original_start_dist);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"adjusted_start_dist:" << adjusted_start_dist);
        double adjusted_end_dist = original_end_dist - length_to_front_bumper_;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"original_end_dist:" << original_end_dist);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"adjusted_end_dist:" << adjusted_end_dist);
        SET_MANEUVER_PROPERTY(maneuver, start_dist, adjusted_start_dist);
        SET_MANEUVER_PROPERTY(maneuver, end_dist, adjusted_end_dist);
 
        // Shift maneuver starting and ending lanelets
        // NOTE: Assumes that maneuver start and end downtrack distances have not been shifted by more than one lanelet
        if(maneuver.type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING && !maneuver.lane_following_maneuver.lane_ids.empty()){
            // (1) Add new beginning lanelet to maneuver if necessary and (2) remove ending lanelet from maneuver if necessary
 
            // Obtain the original starting lanelet from the maneuver
            lanelet::Id original_starting_lanelet_id = std::stoi(maneuver.lane_following_maneuver.lane_ids.front());
            lanelet::ConstLanelet original_starting_lanelet = wm_->getMap()->laneletLayer.get(original_starting_lanelet_id);
 
            // Get the downtrack of the start of the original starting lanelet
            lanelet::BasicPoint2d original_starting_lanelet_centerline_start_point = lanelet::utils::to2D(original_starting_lanelet.centerline()).front();
            double original_starting_lanelet_centerline_start_point_dt = wm_->routeTrackPos(original_starting_lanelet_centerline_start_point).downtrack;
 
            if(adjusted_start_dist < original_starting_lanelet_centerline_start_point_dt){
 
                auto previous_lanelets = wm_->getMapRoutingGraph()->previous(original_starting_lanelet, false);
 
                if(!previous_lanelets.empty()){
 
                    auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(previous_lanelets);
 
                    lanelet::ConstLanelet previous_lanelet_to_add;
 
                    if (llt_on_route_optional){
                        previous_lanelet_to_add = llt_on_route_optional.value();
                    }
                    else{
                        RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting maneuver for lane follow, no previous lanelet found on the shortest path for lanelet "
                            << original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead");
                        previous_lanelet_to_add = previous_lanelets[0];
                    }
 
                    // lane_ids array is ordered by increasing downtrack, so this new starting lanelet is inserted at the front
                    maneuver.lane_following_maneuver.lane_ids.insert(maneuver.lane_following_maneuver.lane_ids.begin(), std::to_string(previous_lanelet_to_add.id()));
 
                    RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Inserted lanelet " << std::to_string(previous_lanelet_to_add.id()) << " to beginning of maneuver.");
                }
                else{
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "No previous lanelet was found for lanelet " << original_starting_lanelet.id());
                }
            }
 
            // Obtain the maneuver ending lanelet
            lanelet::Id original_ending_lanelet_id = std::stoi(maneuver.lane_following_maneuver.lane_ids.back());
            lanelet::ConstLanelet original_ending_lanelet = wm_->getMap()->laneletLayer.get(original_ending_lanelet_id);
 
            // Get the downtrack of the start of the maneuver ending lanelet
            lanelet::BasicPoint2d original_ending_lanelet_centerline_start_point = lanelet::utils::to2D(original_ending_lanelet.centerline()).front();
            double original_ending_lanelet_centerline_start_point_dt = wm_->routeTrackPos(original_ending_lanelet_centerline_start_point).downtrack;
 
            if(adjusted_end_dist < original_ending_lanelet_centerline_start_point_dt){
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Original ending lanelet " << original_ending_lanelet.id() << " removed from lane_ids since the updated maneuver no longer crosses it");
 
                // lane_ids array is ordered by increasing downtrack, so the last element in the array corresponds to the original ending lanelet
                maneuver.lane_following_maneuver.lane_ids.pop_back();
            }
        }
        else if (maneuver.type != carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING){
            // (1) Update starting maneuver lanelet if necessary and (2) Update ending maneuver lanelet if necessary
 
            // Obtain the original starting lanelet from the maneuver
            lanelet::Id original_starting_lanelet_id = std::stoi(getManeuverStartingLaneletId(maneuver));
            lanelet::ConstLanelet original_starting_lanelet = wm_->getMap()->laneletLayer.get(original_starting_lanelet_id);
 
            // Get the downtrack of the start of the lanelet
            lanelet::BasicPoint2d original_starting_lanelet_centerline_start_point = lanelet::utils::to2D(original_starting_lanelet.centerline()).front();
            double original_starting_lanelet_centerline_start_point_dt = wm_->routeTrackPos(original_starting_lanelet_centerline_start_point).downtrack;
 
            if(adjusted_start_dist < original_starting_lanelet_centerline_start_point_dt){
                auto previous_lanelets = wm_->getMapRoutingGraph()->previous(original_starting_lanelet, false);
                if(!previous_lanelets.empty()){
                    auto llt_on_route_optional = wm_->getFirstLaneletOnShortestPath(previous_lanelets);
                    lanelet::ConstLanelet previous_lanelet_to_add;
 
                    if (llt_on_route_optional){
                        previous_lanelet_to_add = llt_on_route_optional.value();
                    }
                    else{
                        RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "When adjusting non-lane follow maneuver, no previous lanelet found on the shortest path for lanelet "
                            << original_starting_lanelet.id() << ". Picking arbitrary lanelet: " << previous_lanelets[0].id() << ", instead");
                        previous_lanelet_to_add = previous_lanelets[0];
                    }
                    setManeuverStartingLaneletId(maneuver, previous_lanelet_to_add.id());
                }
                else{
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "No previous lanelet was found for lanelet " << original_starting_lanelet.id());
                }
            }
 
            // Obtain the original ending lanelet from the maneuver
            lanelet::Id original_ending_lanelet_id = std::stoi(getManeuverEndingLaneletId(maneuver));
            lanelet::ConstLanelet original_ending_lanelet = wm_->getMap()->laneletLayer.get(original_ending_lanelet_id);
 
            // Get the downtrack of the start of the ending lanelet
            lanelet::BasicPoint2d original_ending_lanelet_centerline_start_point = lanelet::utils::to2D(original_ending_lanelet.centerline()).front();
            double original_ending_lanelet_centerline_start_point_dt = wm_->routeTrackPos(original_ending_lanelet_centerline_start_point).downtrack;
 
            if(adjusted_end_dist < original_ending_lanelet_centerline_start_point_dt){
                auto previous_lanelets = wm_->getMapRoutingGraph()->previous(original_ending_lanelet, false);
 
                if(!previous_lanelets.empty()){
                    setManeuverEndingLaneletId(maneuver, previous_lanelets[0].id());
                }
                else{
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "No previous lanelet was found for lanelet " << original_starting_lanelet.id());
                }
            }
        }
    }

References GET_MANEUVER_PROPERTY, plan_delegator::anonymous_namespace{plan_delegator.cpp}::getManeuverEndingLaneletId(), plan_delegator::anonymous_namespace{plan_delegator.cpp}::getManeuverStartingLaneletId(), length_to_front_bumper_, SET_MANEUVER_PROPERTY, plan_delegator::anonymous_namespace{plan_delegator.cpp}::setManeuverEndingLaneletId(), plan_delegator::anonymous_namespace{plan_delegator.cpp}::setManeuverStartingLaneletId(), carma_cooperative_perception::to_string(), and wm_.

Referenced by maneuverPlanCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

◆ config_

Config plan_delegator::PlanDelegator::config_

protected

Definition at line 188 of file plan_delegator.hpp.

Referenced by PlanDelegator(), getPlannerClientByName(), handle_on_activate(), handle_on_configure(), onTrajPlanTick(), planTrajectory(), and publishTurnSignalCommand().

◆ consecutive_traj_gen_failure_num_

int plan_delegator::PlanDelegator::consecutive_traj_gen_failure_num_ = 0

private

Definition at line 220 of file plan_delegator.hpp.

Referenced by onTrajPlanTick().

◆ current_lane_change_information_

boost::optional<LaneChangeInformation> plan_delegator::PlanDelegator::current_lane_change_information_

private

Definition at line 232 of file plan_delegator.hpp.

Referenced by maneuverPlanCallback(), and poseCallback().

◆ guidance_engaged

bool plan_delegator::PlanDelegator::guidance_engaged = false

private

Definition at line 219 of file plan_delegator.hpp.

Referenced by guidanceStateCallback(), onTrajPlanTick(), and planTrajectory().

◆ guidance_state_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> plan_delegator::PlanDelegator::guidance_state_sub_

private

Definition at line 213 of file plan_delegator.hpp.

Referenced by handle_on_configure().

◆ last_successful_traj_

std::optional<carma_planning_msgs::msg::TrajectoryPlan> plan_delegator::PlanDelegator::last_successful_traj_

private

Definition at line 204 of file plan_delegator.hpp.

Referenced by onTrajPlanTick().

◆ latest_maneuver_plan_

carma_planning_msgs::msg::ManeuverPlan plan_delegator::PlanDelegator::latest_maneuver_plan_

protected

Definition at line 193 of file plan_delegator.hpp.

Referenced by maneuverPlanCallback(), and planTrajectory().

◆ latest_pose_

geometry_msgs::msg::PoseStamped plan_delegator::PlanDelegator::latest_pose_

protected

Definition at line 195 of file plan_delegator.hpp.

Referenced by composePlanTrajectoryRequest(), maneuverPlanCallback(), planTrajectory(), poseCallback(), and publishUpcomingLaneChangeStatus().

◆ latest_turn_signal_command_

autoware_msgs::msg::LampCmd plan_delegator::PlanDelegator::latest_turn_signal_command_

private

Definition at line 238 of file plan_delegator.hpp.

Referenced by publishTurnSignalCommand().

◆ latest_twist_

geometry_msgs::msg::TwistStamped plan_delegator::PlanDelegator::latest_twist_

protected

Definition at line 196 of file plan_delegator.hpp.

Referenced by composePlanTrajectoryRequest(), handle_on_configure(), and publishTurnSignalCommand().

◆ length_to_front_bumper_

double plan_delegator::PlanDelegator::length_to_front_bumper_ = 3.0

private

Definition at line 222 of file plan_delegator.hpp.

Referenced by lookupFrontBumperTransform(), and updateManeuverParameters().

◆ MILLISECOND_TO_SECOND

const constexpr double plan_delegator::PlanDelegator::MILLISECOND_TO_SECOND = 0.001

staticconstexpr

Definition at line 115 of file plan_delegator.hpp.

◆ plan_sub_

carma_ros2_utils::SubPtr<carma_planning_msgs::msg::ManeuverPlan> plan_delegator::PlanDelegator::plan_sub_

private

Definition at line 210 of file plan_delegator.hpp.

Referenced by handle_on_configure().

◆ pose_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::PoseStamped> plan_delegator::PlanDelegator::pose_sub_

private

Definition at line 211 of file plan_delegator.hpp.

Referenced by handle_on_configure().

◆ received_maneuver_plan_

bool plan_delegator::PlanDelegator::received_maneuver_plan_ = false

protected

Definition at line 194 of file plan_delegator.hpp.

Referenced by maneuverPlanCallback(), and onTrajPlanTick().

◆ tf2_buffer_

std::shared_ptr<tf2_ros::Buffer> plan_delegator::PlanDelegator::tf2_buffer_

private

Definition at line 225 of file plan_delegator.hpp.

Referenced by lookupFrontBumperTransform().

◆ tf2_listener_

std::shared_ptr<tf2_ros::TransformListener> plan_delegator::PlanDelegator::tf2_listener_

private

Definition at line 226 of file plan_delegator.hpp.

Referenced by lookupFrontBumperTransform().

◆ timer_callback_group_

rclcpp::CallbackGroup::SharedPtr plan_delegator::PlanDelegator::timer_callback_group_

private

Definition at line 217 of file plan_delegator.hpp.

Referenced by handle_on_activate().

◆ traj_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::TrajectoryPlan> plan_delegator::PlanDelegator::traj_pub_

private

Definition at line 205 of file plan_delegator.hpp.

Referenced by handle_on_configure(), and onTrajPlanTick().

◆ traj_timer_

rclcpp::TimerBase::SharedPtr plan_delegator::PlanDelegator::traj_timer_

private

Definition at line 215 of file plan_delegator.hpp.

Referenced by handle_on_activate().

◆ trajectory_planners_

std::unordered_map<std::string, carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> > plan_delegator::PlanDelegator::trajectory_planners_

protected

Definition at line 191 of file plan_delegator.hpp.

Referenced by getPlannerClientByName().

◆ turn_signal_command_pub_

carma_ros2_utils::PubPtr<autoware_msgs::msg::LampCmd> plan_delegator::PlanDelegator::turn_signal_command_pub_

private

Definition at line 207 of file plan_delegator.hpp.

Referenced by handle_on_configure(), and publishTurnSignalCommand().

◆ twist_sub_

carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> plan_delegator::PlanDelegator::twist_sub_

private

Definition at line 212 of file plan_delegator.hpp.

Referenced by handle_on_configure().

◆ upcoming_lane_change_information_

boost::optional<LaneChangeInformation> plan_delegator::PlanDelegator::upcoming_lane_change_information_

private

Definition at line 229 of file plan_delegator.hpp.

Referenced by maneuverPlanCallback(), and poseCallback().

◆ upcoming_lane_change_status_

carma_planning_msgs::msg::UpcomingLaneChangeStatus plan_delegator::PlanDelegator::upcoming_lane_change_status_

private

Definition at line 235 of file plan_delegator.hpp.

Referenced by poseCallback(), and publishUpcomingLaneChangeStatus().

◆ upcoming_lane_change_status_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::UpcomingLaneChangeStatus> plan_delegator::PlanDelegator::upcoming_lane_change_status_pub_

private

Definition at line 206 of file plan_delegator.hpp.

Referenced by handle_on_configure(), and publishUpcomingLaneChangeStatus().

◆ wm_

carma_wm::WorldModelConstPtr plan_delegator::PlanDelegator::wm_

protected

Definition at line 200 of file plan_delegator.hpp.

Referenced by getLaneChangeInformation(), handle_on_configure(), maneuverPlanCallback(), planTrajectory(), publishUpcomingLaneChangeStatus(), and updateManeuverParameters().

◆ wml_

carma_wm::WMListener plan_delegator::PlanDelegator::wml_

protected

Definition at line 199 of file plan_delegator.hpp.

Referenced by handle_on_configure().

The documentation for this class was generated from the following files:

plan_delegator/include/plan_delegator.hpp
plan_delegator/src/plan_delegator.cpp

Public Member Functions

Static Public Attributes

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PlanDelegator()

Member Function Documentation

◆ composePlanTrajectoryRequest()

◆ FRIEND_TEST() [1/5]

◆ FRIEND_TEST() [2/5]

◆ FRIEND_TEST() [3/5]

◆ FRIEND_TEST() [4/5]

◆ FRIEND_TEST() [5/5]

◆ getLaneChangeInformation()

◆ getPlannerClientByName()

◆ guidanceStateCallback()

◆ handle_on_activate()

◆ handle_on_configure()

◆ isManeuverExpired()

◆ isManeuverPlanValid()

◆ isTrajectoryLongEnough()

◆ isTrajectoryValid()

◆ lookupFrontBumperTransform()

◆ maneuverPlanCallback()

◆ onTrajPlanTick()

◆ planTrajectory()

◆ poseCallback()

◆ publishTurnSignalCommand()

◆ publishUpcomingLaneChangeStatus()

◆ updateManeuverParameters()

Member Data Documentation

◆ config_

◆ consecutive_traj_gen_failure_num_

◆ current_lane_change_information_

◆ guidance_engaged

◆ guidance_state_sub_

◆ last_successful_traj_

◆ latest_maneuver_plan_

◆ latest_pose_

◆ latest_turn_signal_command_

◆ latest_twist_

◆ length_to_front_bumper_

◆ MILLISECOND_TO_SECOND

◆ plan_sub_

◆ pose_sub_

◆ received_maneuver_plan_

◆ tf2_buffer_

◆ tf2_listener_

◆ timer_callback_group_

◆ traj_pub_

◆ traj_timer_

◆ trajectory_planners_

◆ turn_signal_command_pub_

◆ twist_sub_

◆ upcoming_lane_change_information_

◆ upcoming_lane_change_status_

◆ upcoming_lane_change_status_pub_

◆ wm_

◆ wml_