plan_delegator::PlanDelegator Class Reference

#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 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_
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
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_
bool	guidance_engaged = false
double	length_to_front_bumper_ = 3.0
tf2_ros::Buffer	tf2_buffer_
std::unique_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 106 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_(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_.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_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 uint16_t &	current_maneuver_index
	)		const

Generate new PlanTrajecory service request based on current planning progress.

Returns

a PlanTrajectoryRequest which is ready to be used in the following service call

Definition at line 414 of file plan_delegator.cpp.

    {
        auto plan_req = std::make_shared<carma_planning_msgs::srv::PlanTrajectory::Request>();
        plan_req->maneuver_plan = latest_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_maneuver_plan_, 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 255 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 375 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 188 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 180 of file plan_delegator.cpp.

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

References config_, onTrajPlanTick(), 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 148 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);
 
        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;
    }

References config_, plan_delegator::Config::duration_to_signal_before_lane_change, carma_wm::WMListener::getWorldModel(), guidance_state_sub_, guidanceStateCallback(), latest_twist_, lookupFrontBumperTransform(), maneuverPlanCallback(), plan_delegator::Config::max_trajectory_duration, plan_delegator::Config::min_crawl_speed, plan_sub_, plan_delegator::Config::planning_topic_prefix, plan_delegator::Config::planning_topic_suffix, pose_sub_, poseCallback(), plan_delegator::Config::tactical_plugin_service_call_timeout, traj_pub_, plan_delegator::Config::trajectory_planning_rate, turn_signal_command_pub_, twist_sub_, upcoming_lane_change_status_pub_, wm_, and wml_.

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 403 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 391 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 455 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 397 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 722 of file plan_delegator.cpp.

    {
        tf2_listener_.reset(new 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 193 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;
 
        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_, 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 706 of file plan_delegator.cpp.

    {
        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();
            traj_pub_->publish(trajectory_plan);
        }
        else
        {
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),"Planned trajectory is empty. It will not be published!");
        }
    }

References isTrajectoryValid(), planTrajectory(), 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 592 of file plan_delegator.cpp.

    {
        carma_planning_msgs::msg::TrajectoryPlan latest_trajectory_plan;
        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;
        }
 
        // 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 < latest_maneuver_plan_.maneuvers.size())
        {
            // const auto& maneuver = latest_maneuver_plan_.maneuvers[current_maneuver_index];
            auto& maneuver = latest_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, current_maneuver_index);
 
            auto plan_response = client->async_send_request(plan_req);
 
            auto future_status = plan_response.wait_for(std::chrono::milliseconds(config_.tactical_plugin_service_call_timeout));
 
            // Wait for the result.
            if (future_status == std::future_status::ready)
            {
                // validate trajectory before add to the plan
                if(!isTrajectoryValid(plan_response.get()->trajectory_plan))
                {
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),"Found invalid trajectory with less than 2 trajectory points for " << std::string(latest_maneuver_plan_.maneuver_plan_id));
                    break;
                }
                //Remove duplicate point from start of trajectory
                if(latest_trajectory_plan.trajectory_points.size() !=0){
 
                    if(latest_trajectory_plan.trajectory_points.back().target_time == plan_response.get()->trajectory_plan.trajectory_points.front().target_time){
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"Removing duplicate point for planner: " << maneuver_planner);
                        plan_response.get()->trajectory_plan.trajectory_points.erase(plan_response.get()->trajectory_plan.trajectory_points.begin());
                        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"),"plan_response.get()->trajectory_plan size: " << plan_response.get()->trajectory_plan.trajectory_points.size());
 
                    }
                }
                latest_trajectory_plan.trajectory_points.insert(latest_trajectory_plan.trajectory_points.end(),
                                                                plan_response.get()->trajectory_plan.trajectory_points.begin(),
                                                                plan_response.get()->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.get()->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(latest_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.get()->related_maneuvers.size() > 0)
                {
                    current_maneuver_index = plan_response.get()->related_maneuvers.back() + 1;
                }
            }
            else
            {
                RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"),"Unsuccessful service call to trajectory planner:" << maneuver_planner << " for plan ID " << std::string(latest_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
                break;
            }
        }
 
        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 244 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 337 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 307 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 461 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 176 of file plan_delegator.hpp.

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

current_lane_change_information_

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

private

Definition at line 215 of file plan_delegator.hpp.

Referenced by maneuverPlanCallback(), and poseCallback().

guidance_engaged

bool plan_delegator::PlanDelegator::guidance_engaged = false

private

Definition at line 203 of file plan_delegator.hpp.

Referenced by guidanceStateCallback(), and planTrajectory().

guidance_state_sub_

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

private

Definition at line 199 of file plan_delegator.hpp.

Referenced by handle_on_configure().

latest_maneuver_plan_

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

protected

Definition at line 181 of file plan_delegator.hpp.

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

latest_pose_

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

protected

Definition at line 182 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 221 of file plan_delegator.hpp.

Referenced by publishTurnSignalCommand().

latest_twist_

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

protected

Definition at line 183 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 205 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 111 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 196 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 197 of file plan_delegator.hpp.

Referenced by handle_on_configure().

tf2_buffer_

tf2_ros::Buffer plan_delegator::PlanDelegator::tf2_buffer_

private

Definition at line 208 of file plan_delegator.hpp.

Referenced by lookupFrontBumperTransform().

tf2_listener_

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

private

Definition at line 209 of file plan_delegator.hpp.

Referenced by lookupFrontBumperTransform().

traj_pub_

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

private

Definition at line 191 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 201 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 179 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 193 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 198 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 212 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 218 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 192 of file plan_delegator.hpp.

Referenced by handle_on_configure(), and publishUpcomingLaneChangeStatus().

wm_

carma_wm::WorldModelConstPtr plan_delegator::PlanDelegator::wm_

protected

Definition at line 187 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 186 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