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 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)
	FRIEND_TEST (TestPlanDelegator, TestGetLaneChangeInformation)

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 132 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 465 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/6]

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

private

FRIEND_TEST() [2/6]

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

private

FRIEND_TEST() [3/6]

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

private

FRIEND_TEST() [4/6]

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

private

FRIEND_TEST() [5/6]

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

private

FRIEND_TEST() [6/6]

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 259 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.
        // This function runs directly inside the final_maneuver_plan subscription callback (once per received
        // plan), so it must never throw here: an uncaught exception in a subscription callback can crash this
        // node outright. Previously it did throw whenever the walk from starting_lanelet to a shared boundary
        // with ending_lanelet hit a lanelet with no routable successor -- which happens whenever an intervening
        // lanelet is closed (e.g. by a TCM) or simply missing adjacency data in the map. Below, that same walk
        // is attempted first since it is exact when it works, but any inability to complete it (no following
        // lanelet, a routing loop) now falls through to a purely geometric left/right estimate instead of
        // failing, so a lane change is always reported instead of crashing.
        boost::optional<bool> is_right_lane_change;
 
        if(starting_lanelet.leftBound() == ending_lanelet.rightBound()){
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Lanelet " << std::to_string(starting_lanelet.id()) << " shares left boundary with " << std::to_string(ending_lanelet.id()));
            is_right_lane_change = false;
        }
        else if(starting_lanelet.rightBound() == ending_lanelet.leftBound()){
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Lanelet " << std::to_string(starting_lanelet.id()) << " shares right boundary with " << std::to_string(ending_lanelet.id()));
            is_right_lane_change = true;
        }
        else
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("plan_delegator"), "Searching for shared boundary with starting lanechange lanelet " << std::to_string(starting_lanelet.id()) << " and ending lanelet " << std::to_string(ending_lanelet.id()));
            lanelet::ConstLanelet current_lanelet = starting_lanelet;
            std::unordered_set<lanelet::Id> visited{current_lanelet.id()};
 
            while(!is_right_lane_change){
                // Assumption: Adjacent lanelets share lane boundary
                auto following_lanelets = wm_->getMapRoutingGraph()->following(current_lanelet, false);
                bool no_successor = following_lanelets.empty();
                lanelet::ConstLanelet candidate_lanelet = no_successor ? current_lanelet : following_lanelets.front();
                bool loop_detected = !no_successor && visited.count(candidate_lanelet.id()) > 0;
 
                if(no_successor)
                {
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "No following lanelets from lanelet " << current_lanelet.id()
                        << " reachable without a lane change (possibly closed or missing from the map); "
                        << "falling back to a geometric left/right estimate for lane change from "
                        << starting_lanelet.id() << " to " << ending_lanelet.id());
                }
 
                if (loop_detected)
                {
                    RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "Detected a routing loop while searching for a shared boundary between lanelet "
                        << starting_lanelet.id() << " and " << ending_lanelet.id() << "; falling back to a geometric left/right estimate");
                }
 
                if(no_successor || loop_detected)
                {
                    break;
                }
 
                current_lanelet = candidate_lanelet;
                visited.insert(current_lanelet.id());
 
                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()));
                if(current_lanelet.leftBound() == ending_lanelet.rightBound()){
                    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()));
                    is_right_lane_change = false;
                }
                else if(current_lanelet.rightBound() == ending_lanelet.leftBound()){
                    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()));
                    is_right_lane_change = true;
                }
            }
        }
 
        if(!is_right_lane_change)
        {
            // Could not confirm a shared boundary via routing (e.g. an intervening lanelet was closed or missing
            // from the map). Fall back to pure geometry: which side of the starting lanelet's heading does the
            // ending lanelet fall on? This only needs the two lanelets' own centerlines, so it works even when
            // they are otherwise disconnected in the routing graph.
            lanelet::BasicLineString2d starting_centerline = starting_lanelet.centerline2d().basicLineString();
            lanelet::BasicPoint2d start_pt = starting_centerline.front();
            lanelet::BasicPoint2d heading_vec = starting_centerline.back() - start_pt;
            lanelet::BasicPoint2d to_target = ending_lanelet.centerline2d().basicLineString().front() - start_pt;
            double cross = heading_vec.x() * to_target.y() - heading_vec.y() * to_target.x();
 
            RCLCPP_WARN_STREAM(rclcpp::get_logger("plan_delegator"), "Could not find a shared lane boundary between starting lanelet "
                << starting_lanelet.id() << " and ending lanelet " << ending_lanelet.id()
                << "; estimating lane change direction geometrically instead.");
 
            is_right_lane_change = (cross < 0.0);
        }
 
        lane_change_information.is_right_lane_change = is_right_lane_change.get();
        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 425 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 192 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 183 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 150 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;
    }

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_traj_generation_reattempt, 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 453 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 441 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 509 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 447 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 827 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 197 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 773 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 646 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 248 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 387 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 357 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 515 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