#include <route_generator_worker.hpp>

Collaboration diagram for route::RouteGeneratorWorker:

Public Member Functions
	RouteGeneratorWorker (tf2_ros::Buffer &tf2_buffer)
	Constructor for RouteGeneratorWorker class taking in dependencies via dependency injection. More...

void	setReroutingChecker (const std::function< bool()> inputFunction)
	setReroutingChecker function to set the rerouting flag More...

void	setWorldModelPtr (carma_wm::WorldModelConstPtr wm)
	Dependency injection for world model pointer. More...

void	setLoggerInterface (rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger)
	Dependency injection for logger interface. More...

void	setClock (rclcpp::Clock::SharedPtr clock)
	Dependency injection for clock object. More...

lanelet::Optional< lanelet::routing::Route >	routing (const lanelet::BasicPoint2d start, const std::vector< lanelet::BasicPoint2d > &via, const lanelet::BasicPoint2d end, const lanelet::LaneletMapConstPtr map_pointer, const carma_wm::LaneletRoutingGraphConstPtr graph_pointer) const
	Generate Lanelet2 route based on input destinations. More...

bool	getAvailableRouteCb (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< carma_planning_msgs::srv::GetAvailableRoutes::Request >, std::shared_ptr< carma_planning_msgs::srv::GetAvailableRoutes::Response > resp)
	Get_available_route service callback. Calls to this service will respond with a list of route names for user to select. More...

bool	setActiveRouteCb (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request > req, std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Response > resp)
	Set_active_route service callback. User can select a route to start following by calling this service. More...

bool	abortActiveRouteCb (const std::shared_ptr< rmw_request_id_t >, const std::shared_ptr< carma_planning_msgs::srv::AbortActiveRoute::Request >, std::shared_ptr< carma_planning_msgs::srv::AbortActiveRoute::Response > resp)
	Abort_active_route service callback. User can call this service to abort a current following route and return to route selection stage. More...

void	bumperPoseCb ()
	Callback for the front bumper pose transform. More...

void	twistCb (geometry_msgs::msg::TwistStamped::UniquePtr msg)
	Callback for the twist subscriber, which will store latest twist locally. More...

void	georeferenceCb (std_msgs::msg::String::UniquePtr msg)
	Callback for the georeference subscriber used to set the map projection. More...

void	setRouteFilePath (const std::string &path)
	Set method for configurable parameter. More...

void	setDowntrackDestinationRange (double dt_dest_range)
	Set method for configurable parameter. More...

void	setPublishers (const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteEvent > &route_event_pub, const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteState > &route_state_pub, const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::Route > &route_pub, const carma_ros2_utils::PubPtr< visualization_msgs::msg::Marker > &route_marker_pub)
	Method to pass publishers into worker class. More...

bool	checkForDuplicateLaneletsInShortestPath (const lanelet::routing::Route &route) const
	Helper function to check whether a route's shortest path contains any duplicate Lanelet IDs. 'true' indicates that the route's shortest path contains duplicate Lanelet IDs. More...

std::vector< lanelet::BasicPoint3d >	loadRouteDestinationsInMapFrame (const std::vector< carma_v2x_msgs::msg::Position3D > &destinations) const
	Function to take route destination points from a vector of 3D points and convert them from lat/lon values to to coordinates in map frame based on the projection string. More...

std::vector< carma_v2x_msgs::msg::Position3D >	loadRouteDestinationGpsPointsFromRouteId (const std::string &route_id) const
	Function to load route destination points from a route file and store them in a vector of 3D points. More...

carma_planning_msgs::msg::Route	composeRouteMsg (const lanelet::Optional< lanelet::routing::Route > &route) const
	Helper function to generate a CARMA route message based on planned lanelet route. More...

bool	spinCallback ()
	Spin callback which will be called frequently based on spin rate. More...

visualization_msgs::msg::Marker	composeRouteMarkerMsg (const lanelet::Optional< lanelet::routing::Route > &route)
	composeRouteMarkerMsg is a function to generate route rviz markers More...

bool	crosstrackErrorCheck (const std::shared_ptr< geometry_msgs::msg::PoseStamped > &msg, lanelet::ConstLanelet current_llt)
	crosstrackErrorCheck is a function that determines when the vehicle has left the route and reports when a crosstrack error has taken place More...

void	setCrosstrackErrorCountMax (int cte_max)
	set the crosstrack error counter maximum limit More...

void	setCrosstrackErrorDistance (double cte_dist)
	set the maximum crosstrack error distance More...

lanelet::ConstLanelet	getClosestLaneletFromRouteLanelets (lanelet::BasicPoint2d position) const
	"Get the closest lanelet on the route relative to the vehicle's current position. If the input list does not contain lanelets on the route, still closest lanelet from the route will be returned More...

void	addLanelet (lanelet::ConstLanelet llt)

lanelet::Optional< lanelet::routing::Route >	rerouteAfterRouteInvalidation (const std::vector< lanelet::BasicPoint2d > &destination_points_in_map)
	After route is invalidated, this function returns a new route based on the destinations points. More...

void	initializeBumperTransformLookup ()
	Initialize transform lookup from front bumper to map. More...

Public Attributes
std::function< bool()>	reroutingChecker
	reroutingChecker function to set the rerouting flag locally More...

boost::optional< geometry_msgs::msg::PoseStamped >	vehicle_pose_

Private Member Functions
void	publishRouteEvent (uint8_t event_type)

Private Attributes
const double	DEG_TO_RAD = 0.0174533

RouteStateWorker	rs_worker_

std::string	route_file_path_

carma_wm::WorldModelConstPtr	world_model_

carma_planning_msgs::msg::Route	route_msg_

carma_planning_msgs::msg::RouteEvent	route_event_msg_

carma_planning_msgs::msg::RouteState	route_state_msg_

visualization_msgs::msg::Marker	route_marker_msg_

std::vector< lanelet::ConstPoint3d >	points_

lanelet::ConstLanelets	route_llts

double	down_track_target_range_

double	current_crosstrack_distance_

double	current_downtrack_distance_

lanelet::BasicPoint2d	current_loc_

double	ll_crosstrack_distance_

double	ll_downtrack_distance_

lanelet::Id	ll_id_

double	speed_limit_ = 0

double	current_speed_ = 0

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteEvent >	route_event_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteState >	route_state_pub_

carma_ros2_utils::PubPtr< carma_planning_msgs::msg::Route >	route_pub_

carma_ros2_utils::PubPtr< visualization_msgs::msg::Marker >	route_marker_pub_

bool	new_route_msg_generated_ = false

bool	new_route_marker_generated_ = false

std::queue< uint8_t >	route_event_queue

double	cross_track_dist_

int	cte_count_ = 0

int	cte_count_max_

std::vector< lanelet::BasicPoint2d >	destination_points_in_map_

boost::optional< std::string >	map_proj_

geometry_msgs::msg::TransformStamped	tf_

tf2::Stamped< tf2::Transform >	frontbumper_transform_

tf2_ros::Buffer &	tf2_buffer_

std::unique_ptr< tf2_ros::TransformListener >	tf2_listener_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger_

rclcpp::Clock::SharedPtr	clock_

Static Private Attributes
static constexpr double	epsilon_ = 0.001

Detailed Description

Definition at line 57 of file route_generator_worker.hpp.

Constructor & Destructor Documentation

◆ RouteGeneratorWorker()

route::RouteGeneratorWorker::RouteGeneratorWorker ( tf2_ros::Buffer & tf2_buffer )

Constructor for RouteGeneratorWorker class taking in dependencies via dependency injection.

Definition at line 21 of file route_generator_worker.cpp.

22 : tf2_buffer_(tf2_buffer) {}

route::RouteGeneratorWorker::tf2_buffer_

tf2_ros::Buffer & tf2_buffer_

Definition: route_generator_worker.hpp:347

Member Function Documentation

◆ abortActiveRouteCb()

bool route::RouteGeneratorWorker::abortActiveRouteCb	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< carma_planning_msgs::srv::AbortActiveRoute::Request >	,
		std::shared_ptr< carma_planning_msgs::srv::AbortActiveRoute::Response >	resp
	)

Abort_active_route service callback. User can call this service to abort a current following route and return to route selection stage.

Parameters

req	A carma_planning_msgs::srv::AbortActiveRoute::Request msg which contains the route name user wants to stop following
resp	A carma_planning_msgs::srv::AbortActiveRoute::Response msg contains error status showing if there is an active route

Definition at line 470 of file route_generator_worker.cpp.

    {
        // only make sense to abort when it is in route following state
        if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::FOLLOWING)
        {
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_ABORTED);
            resp->error_status = carma_planning_msgs::srv::AbortActiveRoute::Response::NO_ERROR;
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_ABORTED);
            route_msg_ = carma_planning_msgs::msg::Route{};
        } else {
            // service call successed but there is not active route
            resp->error_status = carma_planning_msgs::srv::AbortActiveRoute::Response::NO_ACTIVE_ROUTE;
        }
        return true;
    }

References route::RouteStateWorker::getRouteState(), route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), route_msg_, and rs_worker_.

Referenced by route::Route::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ addLanelet()

void route::RouteGeneratorWorker::addLanelet ( lanelet::ConstLanelet llt )

Definition at line 778 of file route_generator_worker.cpp.

    {
        route_llts.push_back(llt);
    }

References route_llts.

◆ bumperPoseCb()

void route::RouteGeneratorWorker::bumperPoseCb ( )

Callback for the front bumper pose transform.

Definition at line 494 of file route_generator_worker.cpp.

    {
        try
        {
            tf_ = tf2_buffer_.lookupTransform("map", "vehicle_front", rclcpp::Time(0,0), rclcpp::Duration(1.0*1e9)); //save to local copy of transform 0.1 sec timeout
            tf2::fromMsg(tf_, frontbumper_transform_);
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_WARN_STREAM(logger_->get_logger(), ex.what());
        }
        
        geometry_msgs::msg::PoseStamped updated_vehicle_pose;
        updated_vehicle_pose.pose.position.x = frontbumper_transform_.getOrigin().getX();
        updated_vehicle_pose.pose.position.y = frontbumper_transform_.getOrigin().getY();
        updated_vehicle_pose.pose.position.z = frontbumper_transform_.getOrigin().getZ();
        vehicle_pose_ = updated_vehicle_pose; 
            
        if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::FOLLOWING) {
            // convert from pose stamp into lanelet basic 2D point
            current_loc_ = lanelet::BasicPoint2d(vehicle_pose_->pose.position.x, vehicle_pose_->pose.position.y);
            // get dt ct from world model
            carma_wm::TrackPos track(0.0, 0.0);
            try {
                track = this->world_model_->routeTrackPos(current_loc_);
            } catch (std::invalid_argument ex) {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "Routing has finished but carma_wm has not receive it!");
                return;
            }
 
            // Return if world model has not yet been updated with the current active route
            if ((this->world_model_->getRouteName()).compare(route_msg_.route_name) != 0) {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "Current active route name is " << route_msg_.route_name << ", WorldModel is using " << this->world_model_->getRouteName());
                return;
            }
 
            auto current_lanelet = getClosestLaneletFromRouteLanelets(current_loc_);
            auto lanelet_track = carma_wm::geometry::trackPos(current_lanelet, current_loc_);
            ll_id_ = current_lanelet.id();
            ll_crosstrack_distance_ = lanelet_track.crosstrack;
            ll_downtrack_distance_ = lanelet_track.downtrack;
            current_crosstrack_distance_ = track.crosstrack;
            current_downtrack_distance_ = track.downtrack;
            // Determine speed limit
            lanelet::Optional<carma_wm::TrafficRulesConstPtr> traffic_rules = world_model_->getTrafficRules();
            
            if (traffic_rules) 
            {
                auto laneletIterator = world_model_->getMap()->laneletLayer.find(ll_id_);
                if (laneletIterator != world_model_->getMap()->laneletLayer.end()) {
                    speed_limit_ = (*traffic_rules)->speedLimit(*laneletIterator).speedLimit.value();
                } 
                else 
                {
                    RCLCPP_ERROR_STREAM(logger_->get_logger(), "Failed to set the current speed limit. The lanelet_id: "
                        << ll_id_ << " could not be matched with a lanelet in the map. The previous speed limit of "
                        << speed_limit_ << " will be used.");
                }
                
            } 
            else 
            {
                RCLCPP_ERROR_STREAM(logger_->get_logger(), "Failed to set the current speed limit. Valid traffic rules object could not be built.");
            }
            std::shared_ptr<geometry_msgs::msg::PoseStamped> pose_ptr(new geometry_msgs::msg::PoseStamped(*vehicle_pose_));
            
            // check if we left the seleted route by cross track error
            if (crosstrackErrorCheck(pose_ptr, current_lanelet))
            {
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_DEPARTED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_DEPARTED);
            }
 
            // check if we reached our destination be remaining down track distance            
            double route_length_2d = world_model_->getRouteEndTrackPos().downtrack;
            if((current_downtrack_distance_ > route_length_2d - down_track_target_range_ && current_speed_ < epsilon_) || (current_downtrack_distance_ > route_length_2d))
            {
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_COMPLETED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED);
            }
        }
    }

References carma_wm::TrackPos::crosstrack, crosstrackErrorCheck(), current_crosstrack_distance_, current_downtrack_distance_, current_loc_, current_speed_, down_track_target_range_, carma_wm::TrackPos::downtrack, epsilon_, frontbumper_transform_, getClosestLaneletFromRouteLanelets(), route::RouteStateWorker::getRouteState(), ll_crosstrack_distance_, ll_downtrack_distance_, ll_id_, logger_, route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), route_msg_, rs_worker_, speed_limit_, tf2_buffer_, tf_, carma_wm::geometry::trackPos(), vehicle_pose_, and world_model_.

Referenced by spinCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ checkForDuplicateLaneletsInShortestPath()

bool route::RouteGeneratorWorker::checkForDuplicateLaneletsInShortestPath ( const lanelet::routing::Route & route ) const

Helper function to check whether a route's shortest path contains any duplicate Lanelet IDs. 'true' indicates that the route's shortest path contains duplicate Lanelet IDs.

Parameters

route Route object from lanelet2 lib routing function

Definition at line 296 of file route_generator_worker.cpp.

    {
        // Create a vector for the lanelet IDs in the shortest path
        std::vector<lanelet::Id> shortest_path_lanelet_ids;
 
        // Iterate through the shortest path to populate shortest_path_lanelet_ids with lanelet IDs
        for(const auto& ll : route.shortestPath())
        {
            shortest_path_lanelet_ids.push_back(ll.id());
        }
 
        // Verify that there are no duplicate lanelet IDs in the shortest path
        std::sort(shortest_path_lanelet_ids.begin(), shortest_path_lanelet_ids.end());
        
        if (std::adjacent_find(shortest_path_lanelet_ids.begin(), shortest_path_lanelet_ids.end()) != shortest_path_lanelet_ids.end())
        {
            // Route's shortest path contains duplicate lanelet IDs
            return true;
        }
 
        // Route's shortest path does not duplicate lanelet IDs
        return false;
    }

Referenced by setActiveRouteCb(), and spinCallback().

Here is the caller graph for this function:

◆ composeRouteMarkerMsg()

visualization_msgs::msg::Marker route::RouteGeneratorWorker::composeRouteMarkerMsg ( const lanelet::Optional< lanelet::routing::Route > & route )

composeRouteMarkerMsg is a function to generate route rviz markers

Parameters

route Route object from lanelet2 lib routing function

Definition at line 384 of file route_generator_worker.cpp.

    {
        std::vector<lanelet::ConstPoint3d> points;
        auto end_point_3d = route.get().getEndPoint();
        auto last_ll = route.get().shortestPath().back();
        double end_point_downtrack = carma_wm::geometry::trackPos(last_ll, {end_point_3d.x(), end_point_3d.y()}).downtrack;
        double lanelet_downtrack = carma_wm::geometry::trackPos(last_ll, last_ll.centerline().back().basicPoint2d()).downtrack;
        // get number of points to display using ratio of the downtracks
        auto points_until_end_point = int (last_ll.centerline().size() * (end_point_downtrack / lanelet_downtrack));
  
        for(const auto& ll : route.get().shortestPath())
        {
            if (ll.id() == last_ll.id())
            {
                for (int i = 0; i < points_until_end_point; i++)
                {
                    points.push_back(ll.centerline()[i]);
                }
                continue;
            }
            for(const auto& pt : ll.centerline())
            {
                points.push_back(pt);
            }
        }
 
        route_marker_msg_.points={};
 
        // create the marker msgs
        visualization_msgs::msg::Marker marker;
        marker.header.frame_id = "map";
        marker.header.stamp = rclcpp::Time();
        marker.type = visualization_msgs::msg::Marker::SPHERE_LIST;//
        marker.action = visualization_msgs::msg::Marker::ADD;
        marker.ns = "route_visualizer";
 
        marker.scale.x = 0.65;
        marker.scale.y = 0.65;
        marker.scale.z = 0.65;
        marker.frame_locked = true;
 
        marker.id = 0;
        marker.color.r = 1.0F;
        marker.color.g = 1.0F;
        marker.color.b = 1.0F;
        marker.color.a = 1.0F;
 
        if (points.empty())
        {
            RCLCPP_WARN_STREAM(logger_->get_logger(), "No central line points! Returning");
            return route_marker_msg_;
        }
 
        for (int i = 0; i < points.size(); i=i+5)
        {
            geometry_msgs::msg::Point temp_point;
            temp_point.x = points[i].x();
            temp_point.y = points[i].y();
            temp_point.z = 1; //to show up on top of the lanelet lines
            
            marker.points.push_back(temp_point);
        }
        new_route_marker_generated_ = true;
        return marker;
    }

References carma_wm::TrackPos::downtrack, process_bag::i, logger_, new_route_marker_generated_, route_marker_msg_, and carma_wm::geometry::trackPos().

Referenced by setActiveRouteCb(), and spinCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeRouteMsg()

carma_planning_msgs::msg::Route route::RouteGeneratorWorker::composeRouteMsg ( const lanelet::Optional< lanelet::routing::Route > & route ) const

Helper function to generate a CARMA route message based on planned lanelet route.

Parameters

route Route object from lanelet2 lib routing function

Definition at line 450 of file route_generator_worker.cpp.

    {
        carma_planning_msgs::msg::Route msg;
        // iterate through the shortest path to populate shortest_path_lanelet_ids
        for(const auto& ll : route.get().shortestPath())
        {
            msg.shortest_path_lanelet_ids.push_back(ll.id());
        }
        // iterate through all lanelet in the route to populate route_path_lanelet_ids
        for(const auto& ll : route.get().laneletSubmap()->laneletLayer)
        {
            msg.route_path_lanelet_ids.push_back(ll.id());
        }
        msg.end_point.x  = route->getEndPoint().x();
        msg.end_point.y  = route->getEndPoint().y();
        msg.end_point.z  = route->getEndPoint().z();
 
        return msg;
    }

Referenced by setActiveRouteCb(), and spinCallback().

Here is the caller graph for this function:

◆ crosstrackErrorCheck()

bool route::RouteGeneratorWorker::crosstrackErrorCheck	(	const std::shared_ptr< geometry_msgs::msg::PoseStamped > &	msg,
		lanelet::ConstLanelet	current_llt
	)

crosstrackErrorCheck is a function that determines when the vehicle has left the route and reports when a crosstrack error has taken place

Parameters

msg	Msg that contains the vehicle's current position
current_llt	The lanelet that the vehicle is currently in

Definition at line 712 of file route_generator_worker.cpp.

    {
        lanelet::BasicPoint2d position;
 
        position.x()= msg->pose.position.x;
        position.y()= msg->pose.position.y;
 
        if(boost::geometry::within(position, current.polygon2d())) //If vehicle is inside current_lanelet, there is no crosstrack error
        {
            cte_count_ = 0;
            return false;
        }
 
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "LLt Polygon Dimensions1: " << current.polygon2d().front().x()<< ", "<< current.polygon2d().front().y());
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "LLt Polygon Dimensions2: " << current.polygon2d().back().x()<< ", "<< current.polygon2d().back().y());
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Distance1: " << boost::geometry::distance(position, current.polygon2d()) << " Max allowed Crosstrack: " << cross_track_dist_ );
    
        if (boost::geometry::distance(position, current.polygon2d()) > cross_track_dist_) //Evaluate lanelet crosstrack distance from vehicle
            {
                cte_count_++;
 
                if(cte_count_ > cte_count_max_) //If the distance exceeds the crosstrack distance a certain number of times, report that the route has been departed
                    {
                        cte_count_ = 0;
                        return true;
                    }
                else 
                    return false;
            }
        else
            {
                cte_count_ = 0;
                return false;
            }
 
    }

References cross_track_dist_, cte_count_, cte_count_max_, and logger_.

Referenced by bumperPoseCb().

Here is the caller graph for this function:

◆ georeferenceCb()

void route::RouteGeneratorWorker::georeferenceCb ( std_msgs::msg::String::UniquePtr msg )

Callback for the georeference subscriber used to set the map projection.

Parameters

msg	The latest georeference

Definition at line 582 of file route_generator_worker.cpp.

    {
        map_proj_ = msg->data;
    }

References map_proj_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ getAvailableRouteCb()

bool route::RouteGeneratorWorker::getAvailableRouteCb	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< carma_planning_msgs::srv::GetAvailableRoutes::Request >	,
		std::shared_ptr< carma_planning_msgs::srv::GetAvailableRoutes::Response >	resp
	)

Get_available_route service callback. Calls to this service will respond with a list of route names for user to select.

Parameters

req	An empty carma_planning_msgs::srv::GetAvailableRoutes::Request
resp	A carma_planning_msgs::srv::GetAvailableRoutes::Response msg contains a list of empty Route messages with only route name populated

Definition at line 76 of file route_generator_worker.cpp.

    {   
        // Return if the the directory specified by route_file_path_ does not exist
        if(!boost::filesystem::exists(boost::filesystem::path(this->route_file_path_)))
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "No directory exists at " << route_file_path_);
            return true;
        }
 
        // Read all route files in the given directory
        boost::filesystem::directory_iterator end_point;
        for(boost::filesystem::directory_iterator itr(boost::filesystem::path(this->route_file_path_)); itr != end_point; ++itr)
        {
            // Skip if the iterator has landed on a folder
            if(boost::filesystem::is_directory(itr->status()))
            {
                continue;
            }
 
            auto full_file_name = itr->path().filename().generic_string();
            carma_planning_msgs::msg::Route route_msg;
 
            // Skip if '.csv' is not found in the file name
            if(full_file_name.find(".csv") == std::string::npos)
            { 
                continue;
            }          
 
            // Assume route files ending with ".csv", before that is the actual route name
            route_msg.route_id = full_file_name.substr(0, full_file_name.find(".csv"));
            std::ifstream fin(itr->path().generic_string());
            std::string dest_name;
            if(fin.is_open())
            {
                while (!fin.eof())
                {
                    std::string temp;
                    std::getline(fin, temp);
                    if(temp != "") dest_name = temp;
                }
                fin.close();
            } 
            else
            {
                RCLCPP_ERROR_STREAM(logger_->get_logger(), "File open failed...");
            }
            auto last_comma = dest_name.find_last_of(',');
            if(!std::isdigit(dest_name.substr(last_comma + 1).at(0)))
            {
                route_msg.route_name = dest_name.substr(last_comma + 1);
                resp->available_routes.push_back(move(route_msg));
            }
        }
            
        //after route path object is available to select, worker will able to transit state and provide route selection service
        if(this->rs_worker_.getRouteState() == RouteStateWorker::RouteState::LOADING) 
        {
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_LOADED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED);
        }
 
        return true;
    }

References route::RouteStateWorker::getRouteState(), logger_, route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), route_file_path_, and rs_worker_.

Referenced by route::Route::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getClosestLaneletFromRouteLanelets()

lanelet::ConstLanelet route::RouteGeneratorWorker::getClosestLaneletFromRouteLanelets ( lanelet::BasicPoint2d position ) const

"Get the closest lanelet on the route relative to the vehicle's current position. If the input list does not contain lanelets on the route, still closest lanelet from the route will be returned

Parameters

position the current position of the vehicle

Definition at line 751 of file route_generator_worker.cpp.

    {
        double min = std::numeric_limits<double>::infinity();
        lanelet::ConstLanelet min_llt;
        for (const auto& i: route_llts)
         {
            double dist = boost::geometry::distance(position, i.polygon2d());
            if (dist < min)
            {
                min = dist;
                min_llt = i;
            }
     }
    return min_llt;
    }

References process_bag::i, and route_llts.

Referenced by bumperPoseCb().

Here is the caller graph for this function:

◆ initializeBumperTransformLookup()

void route::RouteGeneratorWorker::initializeBumperTransformLookup ( )

Initialize transform lookup from front bumper to map.

Definition at line 488 of file route_generator_worker.cpp.

    {
        tf2_listener_.reset(new tf2_ros::TransformListener(tf2_buffer_));
        tf2_buffer_.setUsingDedicatedThread(true);
    }

References tf2_buffer_, and tf2_listener_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ loadRouteDestinationGpsPointsFromRouteId()

std::vector< carma_v2x_msgs::msg::Position3D > route::RouteGeneratorWorker::loadRouteDestinationGpsPointsFromRouteId ( const std::string & route_id ) const

Function to load route destination points from a route file and store them in a vector of 3D points.

Parameters

route_id This function will read the route file with provided route_id

Definition at line 351 of file route_generator_worker.cpp.

    {
        // compose full path of the route file
        std::string route_file_name = route_file_path_ + route_id + ".csv";
        std::ifstream fs(route_file_name);
        std::string line;
        
        // read each line in route file (if any)
        std::vector<carma_v2x_msgs::msg::Position3D> destination_points;
        while(std::getline(fs, line))
        {
            carma_v2x_msgs::msg::Position3D gps_point;
 
            // lat lon and elev is seperated by comma
            auto comma = line.find(",");
            // convert lon value in degrees from string
            gps_point.longitude = std::stof(line.substr(0, comma));
            line.erase(0, comma + 1);
            comma = line.find(",");
            // convert lat value in degrees from string
            gps_point.latitude = std::stof(line.substr(0, comma));
            // elevation is in meters
            line.erase(0, comma + 1);
            comma = line.find(",");
            gps_point.elevation = std::stof(line.substr(0, comma));
            gps_point.elevation_exists = true;
 
            destination_points.push_back(gps_point);
        }
 
        return destination_points;
    }

References route_file_path_.

Referenced by setActiveRouteCb().

Here is the caller graph for this function:

◆ loadRouteDestinationsInMapFrame()

std::vector< lanelet::BasicPoint3d > route::RouteGeneratorWorker::loadRouteDestinationsInMapFrame ( const std::vector< carma_v2x_msgs::msg::Position3D > & destinations ) const

Function to take route destination points from a vector of 3D points and convert them from lat/lon values to to coordinates in map frame based on the projection string.

Parameters

destinations A vector of carma_v2x_msgs::msg::Position3D points containing destination points provided as lat/long values

Definition at line 320 of file route_generator_worker.cpp.

    {
        if (!map_proj_) {
            throw std::invalid_argument("loadRouteDestinationsInMapFrame (using destination points array) before map projection was set");
        }
        
        lanelet::projection::LocalFrameProjector projector(map_proj_.get().c_str()); // Build map projector
 
        // Process each point in 'destinations'
        std::vector<lanelet::BasicPoint3d> destination_points;
        for (const auto& destination : destinations)
        {
            lanelet::GPSPoint coordinate;
            coordinate.lon = destination.longitude;
            coordinate.lat = destination.latitude;
            
            if(destination.elevation_exists)
            {
                coordinate.ele = destination.elevation;
            }
            else
            {
                coordinate.ele = 0.0;
            }
 
            destination_points.emplace_back(projector.forward(coordinate));
        }
 
        return destination_points;
    }

References map_proj_.

Referenced by setActiveRouteCb().

Here is the caller graph for this function:

◆ publishRouteEvent()

void route::RouteGeneratorWorker::publishRouteEvent ( uint8_t event_type )

private

Definition at line 603 of file route_generator_worker.cpp.

    {
        route_event_queue.push(event_type);
    }

References route_event_queue.

Referenced by abortActiveRouteCb(), bumperPoseCb(), getAvailableRouteCb(), setActiveRouteCb(), setRouteFilePath(), and spinCallback().

Here is the caller graph for this function:

◆ rerouteAfterRouteInvalidation()

lanelet::Optional< lanelet::routing::Route > route::RouteGeneratorWorker::rerouteAfterRouteInvalidation ( const std::vector< lanelet::BasicPoint2d > & destination_points_in_map )

After route is invalidated, this function returns a new route based on the destinations points.

Parameters

destination_points_in_map vector of destination points

Note: Destination points will be removed if the current pose is past those points.

Definition at line 608 of file route_generator_worker.cpp.

    {
        std::vector<lanelet::BasicPoint2d> destination_points_in_map_temp;
        
        for(const auto &i:destination_points_in_map) // Identify all route points that we have not yet passed
        {
            double destination_down_track=world_model_->routeTrackPos(i).downtrack;
            
            if( current_downtrack_distance_< destination_down_track)
            {
                destination_points_in_map_temp.push_back(i);
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "current_downtrack_distance_:" << current_downtrack_distance_);
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "destination_down_track:" << destination_down_track);
            }
        }  
        
        destination_points_in_map_ = destination_points_in_map_temp; // Update our route point list
        
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "New destination_points_in_map.size:" << destination_points_in_map_.size());
 
        auto route=routing(current_loc_, // Route from current location through future destinations
                            std::vector<lanelet::BasicPoint2d>(destination_points_in_map_.begin(), destination_points_in_map_.end() - 1),
                            destination_points_in_map_.back(),
                            world_model_->getMap(), world_model_->getMapRoutingGraph());
 
        return route;
    }

References current_downtrack_distance_, current_loc_, destination_points_in_map_, process_bag::i, logger_, routing(), and world_model_.

Referenced by spinCallback().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ routing()

lanelet::Optional< lanelet::routing::Route > route::RouteGeneratorWorker::routing	(	const lanelet::BasicPoint2d	start,
		const std::vector< lanelet::BasicPoint2d > &	via,
		const lanelet::BasicPoint2d	end,
		const lanelet::LaneletMapConstPtr	map_pointer,
		const carma_wm::LaneletRoutingGraphConstPtr	graph_pointer
	)		const

Generate Lanelet2 route based on input destinations.

Parameters

start	Lanelet 2D point in map frame indicates the starting point of selected route
via	A vector of lanelet 2D points in map frame which contains points we want to visit along the route
end	Lanelet 2D point in map frame indicates the final destination of selected route
map_pointer	A constant pointer to lanelet vector map
graph_pointer	A constant pointer to lanelet vector map routing graph

Definition at line 40 of file route_generator_worker.cpp.

    {
        // find start lanelet
        auto start_lanelet_vector = lanelet::geometry::findNearest(map_pointer->laneletLayer, start, 1);
        // check if there are any lanelets in the map
        if(start_lanelet_vector.empty())
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "Found no lanelets in the map. Routing cannot be done.");
            return lanelet::Optional<lanelet::routing::Route>();
        }
        // extract starting lanelet
        auto start_lanelet = lanelet::ConstLanelet(start_lanelet_vector[0].second.constData());
        // find end lanelet
        auto end_lanelet_vector = lanelet::geometry::findNearest(map_pointer->laneletLayer, end, 1);
        // extract end lanelet
        auto end_lanelet = lanelet::ConstLanelet(end_lanelet_vector[0].second.constData());
        // find all via lanelets
        lanelet::ConstLanelets via_lanelets_vector;
        for(lanelet::BasicPoint2d point : via)
        {
            auto via_lanelet_vector = lanelet::geometry::findNearest(map_pointer->laneletLayer, point, 1);
            via_lanelets_vector.emplace_back(lanelet::ConstLanelet(via_lanelet_vector[0].second.constData()));
        }
        // routing
        return graph_pointer->getRouteVia(start_lanelet, via_lanelets_vector, end_lanelet);
    }

References logger_, and process_traj_logs::point.

Referenced by rerouteAfterRouteInvalidation(), and setActiveRouteCb().

Here is the caller graph for this function:

◆ setActiveRouteCb()

bool route::RouteGeneratorWorker::setActiveRouteCb	(	const std::shared_ptr< rmw_request_id_t >	,
		const std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request >	req,
		std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Response >	resp
	)

Set_active_route service callback. User can select a route to start following by calling this service.

Parameters

req	A carma_planning_msgs::srv::SetActiveRoute::Request msg which contains either a route name a user wants to select or an array of carma_v2x_msgs::msg::Position3D destination points to generate a route from.
resp	A carma_planning_msgs::srv::SetActiveRoute::Response msg contains error status indicating whether the routing succeeded

Definition at line 150 of file route_generator_worker.cpp.

    {   
        // only allow a new route to be activated in route selection state
        if(this->rs_worker_.getRouteState() != RouteStateWorker::RouteState::SELECTION)
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "System is already following a route.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ALREADY_FOLLOWING_ROUTE;
 
            return true;
        }
 
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Valid state proceeding with selection");
            
        // entering to routing state once destinations are picked
        this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_SELECTED);
        publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_SELECTED);
 
        if (!vehicle_pose_) {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "No vehicle position. Routing cannot be completed.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTING_FAILURE;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        // Check if the map projection is available
        if (!map_proj_) {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "Could not generate route as there was no map projection available");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTING_FAILURE;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        // load destination points in map frame
        std::vector<lanelet::BasicPoint3d> destination_points;
        if(req->choice == carma_planning_msgs::srv::SetActiveRoute::Request::ROUTE_ID)
        {   
            RCLCPP_INFO_STREAM(logger_->get_logger(), "set_active_route_cb: Selected Route ID: " << req->route_id);
            std::vector<carma_v2x_msgs::msg::Position3D> gps_destination_points = loadRouteDestinationGpsPointsFromRouteId(req->route_id);
            destination_points = loadRouteDestinationsInMapFrame(gps_destination_points);
        }
        else if(req->choice == carma_planning_msgs::srv::SetActiveRoute::Request::DESTINATION_POINTS_ARRAY)
        {
            RCLCPP_INFO_STREAM(logger_->get_logger(), "set_active_route_cb: Destination Points array of size " << req->destination_points.size());
            destination_points = loadRouteDestinationsInMapFrame(req->destination_points);
        }
 
        // Check that the requested route is valid with at least one destination point
        if(destination_points.size() < 1)
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "Provided route contains no destination points. Routing cannot be completed.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTE_FILE_ERROR;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        if (!world_model_ || !world_model_->getMap()) {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "World model has not been initialized.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTING_FAILURE;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        // convert points in 2d to map frame
        destination_points_in_map_ = lanelet::utils::transform(destination_points, [](auto a) { return lanelet::traits::to2D(a); });
 
        // Add vehicle as first destination point
        auto destination_points_in_map_with_vehicle = destination_points_in_map_;
            
        lanelet::BasicPoint2d vehicle_position(vehicle_pose_->pose.position.x, vehicle_pose_->pose.position.y);
        destination_points_in_map_with_vehicle.insert(destination_points_in_map_with_vehicle.begin(), vehicle_position);
 
        int idx = 0;
        // validate if the points are geometrically in the map
        for (auto pt : destination_points_in_map_with_vehicle)
        {
            if ((world_model_->getLaneletsFromPoint(pt, 1)).empty())
            {
                RCLCPP_ERROR_STREAM(logger_->get_logger(), "Route Generator: " << idx 
                        << "th destination point is not in the map, x: " << pt.x() << " y: " << pt.y());
                resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTE_FILE_ERROR;
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
                return true;
            }
            idx ++;
        }
            
        // get route graph from world model object
        auto p = world_model_->getMapRoutingGraph();
        // generate a route
        auto route = routing(destination_points_in_map_with_vehicle.front(),
                            std::vector<lanelet::BasicPoint2d>(destination_points_in_map_with_vehicle.begin() + 1, destination_points_in_map_with_vehicle.end() - 1),
                            destination_points_in_map_with_vehicle.back(),
                            world_model_->getMap(), world_model_->getMapRoutingGraph());
        // check if route succeeded
        if(!route)
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "Cannot find a route passing all destinations.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTING_FAILURE;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        if (checkForDuplicateLaneletsInShortestPath(route.get()))
        {
            RCLCPP_ERROR_STREAM(logger_->get_logger(), "At least one duplicate Lanelet ID occurs in the shortest path. Routing cannot be completed.");
            resp->error_status = carma_planning_msgs::srv::SetActiveRoute::Response::ROUTING_FAILURE;
            this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
            publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
            return true;
        }
 
        // Specify the end point of the route that is inside the last lanelet
        lanelet::Point3d end_point{lanelet::utils::getId(), destination_points_in_map_with_vehicle.back().x(), destination_points_in_map_with_vehicle.back().y(), 0};
 
        route->setEndPoint(end_point);
 
        // update route message
        route_msg_ = composeRouteMsg(route);
 
        for(auto id : route_msg_.route_path_lanelet_ids)
        {
            auto ll = world_model_->getMap()->laneletLayer.get(id);
            route_llts.push_back(ll);
        }
 
        route_msg_.route_name = req->route_id;
        route_marker_msg_ = composeRouteMarkerMsg(route);
        route_msg_.header.stamp = clock_->now();
        route_msg_.header.frame_id = "map";
        route_msg_.map_version = world_model_->getMapVersion();
        // since routing is done correctly, transit to route following state
        this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_STARTED);
        publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_STARTED);
        // set publish flag such that updated msg will be published in the next spin
        new_route_msg_generated_ = true;
        return true;
    }

References checkForDuplicateLaneletsInShortestPath(), clock_, composeRouteMarkerMsg(), composeRouteMsg(), destination_points_in_map_, route::RouteStateWorker::getRouteState(), loadRouteDestinationGpsPointsFromRouteId(), loadRouteDestinationsInMapFrame(), logger_, map_proj_, new_route_msg_generated_, route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), route_llts, route_marker_msg_, route_msg_, routing(), rs_worker_, vehicle_pose_, and world_model_.

Referenced by route::Route::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setClock()

void route::RouteGeneratorWorker::setClock ( rclcpp::Clock::SharedPtr clock )

Dependency injection for clock object.

Parameters

clock Clock object that will be used by worker class

Definition at line 35 of file route_generator_worker.cpp.

    {
        this->clock_ = clock;
    }

References clock_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setCrosstrackErrorCountMax()

void route::RouteGeneratorWorker::setCrosstrackErrorCountMax ( int cte_max )

set the crosstrack error counter maximum limit

Parameters

cte_max the maximum amount of acceptable crosstrack error instances

Definition at line 772 of file route_generator_worker.cpp.

    {
        cte_count_max_ = cte_max;
 
    }

References cte_count_max_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setCrosstrackErrorDistance()

void route::RouteGeneratorWorker::setCrosstrackErrorDistance ( double cte_dist )

set the maximum crosstrack error distance

Parameters

cte_dist maximum distance value (specified in the this package's parameters.yaml configuration file)

Definition at line 767 of file route_generator_worker.cpp.

    {
        cross_track_dist_ = cte_dist;
    }

References cross_track_dist_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setDowntrackDestinationRange()

void route::RouteGeneratorWorker::setDowntrackDestinationRange ( double dt_dest_range )

Set method for configurable parameter.

Parameters

dt_dest_range Minimum down track error which can trigger route complete event

Definition at line 598 of file route_generator_worker.cpp.

    {
        this->down_track_target_range_ = dt_dest_range;
    }

References down_track_target_range_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setLoggerInterface()

void route::RouteGeneratorWorker::setLoggerInterface ( rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger )

Dependency injection for logger interface.

Parameters

logger Logger interface that will be used by worker class

Definition at line 29 of file route_generator_worker.cpp.

    {
        this->logger_ = logger;
        this->rs_worker_.setLoggerInterface(logger);
    }

References logger_, rs_worker_, and route::RouteStateWorker::setLoggerInterface().

Referenced by route::Route::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setPublishers()

void route::RouteGeneratorWorker::setPublishers	(	const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteEvent > &	route_event_pub,
		const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::RouteState > &	route_state_pub,
		const carma_ros2_utils::PubPtr< carma_planning_msgs::msg::Route > &	route_pub,
		const carma_ros2_utils::PubPtr< visualization_msgs::msg::Marker > &	route_marker_pub
	)

Method to pass publishers into worker class.

Parameters

route_event_pub	Route event publisher
route_state_pub	Route state publisher
route_pub	Route publisher
route_marker_pub	Route marker publisher

Definition at line 587 of file route_generator_worker.cpp.

    {
        route_event_pub_ = route_event_pub;
        route_state_pub_ = route_state_pub;
        route_pub_ = route_pub;
        route_marker_pub_= route_marker_pub;
    }

References route_event_pub_, route_marker_pub_, route_pub_, and route_state_pub_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setReroutingChecker()

void route::RouteGeneratorWorker::setReroutingChecker ( const std::function< bool()> inputFunction )

setReroutingChecker function to set the rerouting flag

Definition at line 71 of file route_generator_worker.cpp.

    {
        reroutingChecker=inputFunction;
    }

References reroutingChecker.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ setRouteFilePath()

void route::RouteGeneratorWorker::setRouteFilePath ( const std::string & path )

Set method for configurable parameter.

Parameters

path	The location of route files

Definition at line 142 of file route_generator_worker.cpp.

    {
        this->route_file_path_ = path;
        // after route path is set, worker will able to transit state and provide route selection service
        this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_LOADED);
        publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED);
    }

References route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), route_file_path_, and rs_worker_.

Referenced by route::Route::handle_on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setWorldModelPtr()

void route::RouteGeneratorWorker::setWorldModelPtr ( carma_wm::WorldModelConstPtr wm )

Dependency injection for world model pointer.

Parameters

wm	CARMA world model object providing lanelet vector map and traffic regulations

Definition at line 24 of file route_generator_worker.cpp.

    {
        this->world_model_ = wm;
    }

References world_model_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

◆ spinCallback()

bool route::RouteGeneratorWorker::spinCallback ( )

Spin callback which will be called frequently based on spin rate.

Definition at line 636 of file route_generator_worker.cpp.

    {
        // Update vehicle position
        bumperPoseCb();
 
        if(reroutingChecker()==true)
        {
           RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Rerouting required");
           this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_INVALIDATION);
           publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_INVALIDATION);
           auto route = rerouteAfterRouteInvalidation(destination_points_in_map_);
 
           // check if route successed
           if(!route)
            {
                RCLCPP_ERROR_STREAM(logger_->get_logger(), "Cannot find a route passing all destinations.");
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
                return true;
            }
            else if(checkForDuplicateLaneletsInShortestPath(route.get()))
            {
                RCLCPP_ERROR_STREAM(logger_->get_logger(), "At least one duplicate Lanelet ID occurs in the shortest path. Routing cannot be completed.");
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_GEN_FAILED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_GEN_FAILED);
                return true;
            }
            else
            {
                this->rs_worker_.onRouteEvent(RouteStateWorker::RouteEvent::ROUTE_STARTED);
                publishRouteEvent(carma_planning_msgs::msg::RouteEvent::ROUTE_STARTED);  
            }    
            std::string original_route_name = route_msg_.route_name;
            route_msg_ = composeRouteMsg(route);
            route_msg_.route_name = original_route_name;
            route_msg_.is_rerouted = true;
            route_msg_.map_version = world_model_->getMapVersion();
            route_marker_msg_ = composeRouteMarkerMsg(route);
            new_route_msg_generated_ = true;
            new_route_marker_generated_ = true;
        }
        
        // publish new route and set new route flag back to false
        if(new_route_msg_generated_ && new_route_marker_generated_)
        {
            route_pub_->publish(route_msg_);
            route_marker_pub_->publish(route_marker_msg_);
            new_route_msg_generated_ = false;
            new_route_marker_generated_ = false;
            route_msg_.is_rerouted = false;
        }
        // publish route state messsage if a route is selected
        if(route_msg_.route_name != "")
        {
            carma_planning_msgs::msg::RouteState state_msg;
            state_msg.header.stamp = clock_->now();
            state_msg.route_id = route_msg_.route_name;
            state_msg.cross_track = current_crosstrack_distance_;
            state_msg.down_track = current_downtrack_distance_;
            state_msg.lanelet_downtrack = ll_downtrack_distance_;            
            state_msg.state = this->rs_worker_.getRouteState();
            state_msg.lanelet_id = ll_id_;
            state_msg.speed_limit = speed_limit_;
            route_state_pub_->publish(state_msg);
        }
        // publish route event in order if any
        while(!route_event_queue.empty())
        {
            route_event_msg_.event = route_event_queue.front();
            RCLCPP_INFO_STREAM(logger_->get_logger(), "Publishing a route event!");
            route_event_pub_->publish(route_event_msg_);
            route_event_queue.pop();
        }
        return true; 
    }

References bumperPoseCb(), checkForDuplicateLaneletsInShortestPath(), clock_, composeRouteMarkerMsg(), composeRouteMsg(), current_crosstrack_distance_, current_downtrack_distance_, destination_points_in_map_, route::RouteStateWorker::getRouteState(), ll_downtrack_distance_, ll_id_, logger_, new_route_marker_generated_, new_route_msg_generated_, route::RouteStateWorker::onRouteEvent(), publishRouteEvent(), rerouteAfterRouteInvalidation(), reroutingChecker, route_event_msg_, route_event_pub_, route_event_queue, route_marker_msg_, route_marker_pub_, route_msg_, route_pub_, route_state_pub_, rs_worker_, speed_limit_, and world_model_.

Referenced by route::Route::handle_on_activate().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ twistCb()

void route::RouteGeneratorWorker::twistCb ( geometry_msgs::msg::TwistStamped::UniquePtr msg )

Callback for the twist subscriber, which will store latest twist locally.

Parameters

msg	Latest twist message

Definition at line 577 of file route_generator_worker.cpp.

    {
        current_speed_ = msg->twist.linear.x;
    }

References current_speed_.

Referenced by route::Route::handle_on_configure().

Here is the caller graph for this function:

Member Data Documentation

◆ clock_

rclcpp::Clock::SharedPtr route::RouteGeneratorWorker::clock_

private

Definition at line 353 of file route_generator_worker.hpp.

Referenced by setActiveRouteCb(), setClock(), and spinCallback().

◆ cross_track_dist_

double route::RouteGeneratorWorker::cross_track_dist_

private

Definition at line 330 of file route_generator_worker.hpp.

Referenced by crosstrackErrorCheck(), and setCrosstrackErrorDistance().

◆ cte_count_

int route::RouteGeneratorWorker::cte_count_ = 0

private

Definition at line 333 of file route_generator_worker.hpp.

Referenced by crosstrackErrorCheck().

◆ cte_count_max_

int route::RouteGeneratorWorker::cte_count_max_

private

Definition at line 335 of file route_generator_worker.hpp.

Referenced by crosstrackErrorCheck(), and setCrosstrackErrorCountMax().

◆ current_crosstrack_distance_

double route::RouteGeneratorWorker::current_crosstrack_distance_

private

Definition at line 290 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and spinCallback().

◆ current_downtrack_distance_

double route::RouteGeneratorWorker::current_downtrack_distance_

private

Definition at line 293 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), rerouteAfterRouteInvalidation(), and spinCallback().

◆ current_loc_

lanelet::BasicPoint2d route::RouteGeneratorWorker::current_loc_

private

Definition at line 296 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and rerouteAfterRouteInvalidation().

◆ current_speed_

double route::RouteGeneratorWorker::current_speed_ = 0

private

Definition at line 309 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and twistCb().

◆ DEG_TO_RAD

const double route::RouteGeneratorWorker::DEG_TO_RAD = 0.0174533

private

Definition at line 264 of file route_generator_worker.hpp.

◆ destination_points_in_map_

std::vector<lanelet::BasicPoint2d> route::RouteGeneratorWorker::destination_points_in_map_

private

Definition at line 338 of file route_generator_worker.hpp.

Referenced by rerouteAfterRouteInvalidation(), setActiveRouteCb(), and spinCallback().

◆ down_track_target_range_

double route::RouteGeneratorWorker::down_track_target_range_

private

Definition at line 287 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and setDowntrackDestinationRange().

◆ epsilon_

constexpr double route::RouteGeneratorWorker::epsilon_ = 0.001

staticconstexprprivate

Definition at line 311 of file route_generator_worker.hpp.

Referenced by bumperPoseCb().

◆ frontbumper_transform_

tf2::Stamped<tf2::Transform> route::RouteGeneratorWorker::frontbumper_transform_

private

Definition at line 344 of file route_generator_worker.hpp.

Referenced by bumperPoseCb().

◆ ll_crosstrack_distance_

double route::RouteGeneratorWorker::ll_crosstrack_distance_

private

Definition at line 299 of file route_generator_worker.hpp.

Referenced by bumperPoseCb().

◆ ll_downtrack_distance_

double route::RouteGeneratorWorker::ll_downtrack_distance_

private

Definition at line 302 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and spinCallback().

◆ ll_id_

lanelet::Id route::RouteGeneratorWorker::ll_id_

private

Definition at line 304 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and spinCallback().

◆ logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr route::RouteGeneratorWorker::logger_

private

Definition at line 351 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), composeRouteMarkerMsg(), crosstrackErrorCheck(), getAvailableRouteCb(), rerouteAfterRouteInvalidation(), routing(), setActiveRouteCb(), setLoggerInterface(), and spinCallback().

◆ map_proj_

boost::optional<std::string> route::RouteGeneratorWorker::map_proj_

private

Definition at line 341 of file route_generator_worker.hpp.

Referenced by georeferenceCb(), loadRouteDestinationsInMapFrame(), and setActiveRouteCb().

◆ new_route_marker_generated_

bool route::RouteGeneratorWorker::new_route_marker_generated_ = false

private

Definition at line 321 of file route_generator_worker.hpp.

Referenced by composeRouteMarkerMsg(), and spinCallback().

◆ new_route_msg_generated_

bool route::RouteGeneratorWorker::new_route_msg_generated_ = false

private

Definition at line 320 of file route_generator_worker.hpp.

Referenced by setActiveRouteCb(), and spinCallback().

◆ points_

std::vector<lanelet::ConstPoint3d> route::RouteGeneratorWorker::points_

private

Definition at line 281 of file route_generator_worker.hpp.

◆ reroutingChecker

std::function<bool()> route::RouteGeneratorWorker::reroutingChecker

reroutingChecker function to set the rerouting flag locally

Definition at line 69 of file route_generator_worker.hpp.

Referenced by setReroutingChecker(), and spinCallback().

◆ route_event_msg_

carma_planning_msgs::msg::RouteEvent route::RouteGeneratorWorker::route_event_msg_

private

Definition at line 277 of file route_generator_worker.hpp.

Referenced by spinCallback().

◆ route_event_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::RouteEvent> route::RouteGeneratorWorker::route_event_pub_

private

Definition at line 314 of file route_generator_worker.hpp.

Referenced by setPublishers(), and spinCallback().

◆ route_event_queue

std::queue<uint8_t> route::RouteGeneratorWorker::route_event_queue

private

Definition at line 324 of file route_generator_worker.hpp.

Referenced by publishRouteEvent(), and spinCallback().

◆ route_file_path_

std::string route::RouteGeneratorWorker::route_file_path_

private

Definition at line 270 of file route_generator_worker.hpp.

Referenced by getAvailableRouteCb(), loadRouteDestinationGpsPointsFromRouteId(), and setRouteFilePath().

◆ route_llts

lanelet::ConstLanelets route::RouteGeneratorWorker::route_llts

private

Definition at line 284 of file route_generator_worker.hpp.

Referenced by addLanelet(), getClosestLaneletFromRouteLanelets(), and setActiveRouteCb().

◆ route_marker_msg_

visualization_msgs::msg::Marker route::RouteGeneratorWorker::route_marker_msg_

private

Definition at line 279 of file route_generator_worker.hpp.

Referenced by composeRouteMarkerMsg(), setActiveRouteCb(), and spinCallback().

◆ route_marker_pub_

carma_ros2_utils::PubPtr<visualization_msgs::msg::Marker> route::RouteGeneratorWorker::route_marker_pub_

private

Definition at line 317 of file route_generator_worker.hpp.

Referenced by setPublishers(), and spinCallback().

◆ route_msg_

carma_planning_msgs::msg::Route route::RouteGeneratorWorker::route_msg_

private

Definition at line 276 of file route_generator_worker.hpp.

Referenced by abortActiveRouteCb(), bumperPoseCb(), setActiveRouteCb(), and spinCallback().

◆ route_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::Route> route::RouteGeneratorWorker::route_pub_

private

Definition at line 316 of file route_generator_worker.hpp.

Referenced by setPublishers(), and spinCallback().

◆ route_state_msg_

carma_planning_msgs::msg::RouteState route::RouteGeneratorWorker::route_state_msg_

private

Definition at line 278 of file route_generator_worker.hpp.

◆ route_state_pub_

carma_ros2_utils::PubPtr<carma_planning_msgs::msg::RouteState> route::RouteGeneratorWorker::route_state_pub_

private

Definition at line 315 of file route_generator_worker.hpp.

Referenced by setPublishers(), and spinCallback().

◆ rs_worker_

RouteStateWorker route::RouteGeneratorWorker::rs_worker_

private

Definition at line 267 of file route_generator_worker.hpp.

Referenced by abortActiveRouteCb(), bumperPoseCb(), getAvailableRouteCb(), setActiveRouteCb(), setLoggerInterface(), setRouteFilePath(), and spinCallback().

◆ speed_limit_

double route::RouteGeneratorWorker::speed_limit_ = 0

private

Definition at line 307 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and spinCallback().

◆ tf2_buffer_

tf2_ros::Buffer& route::RouteGeneratorWorker::tf2_buffer_

private

Definition at line 347 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and initializeBumperTransformLookup().

◆ tf2_listener_

std::unique_ptr<tf2_ros::TransformListener> route::RouteGeneratorWorker::tf2_listener_

private

Definition at line 348 of file route_generator_worker.hpp.

Referenced by initializeBumperTransformLookup().

◆ tf_

geometry_msgs::msg::TransformStamped route::RouteGeneratorWorker::tf_

private

Definition at line 343 of file route_generator_worker.hpp.

Referenced by bumperPoseCb().

◆ vehicle_pose_

boost::optional<geometry_msgs::msg::PoseStamped> route::RouteGeneratorWorker::vehicle_pose_

Definition at line 260 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), and setActiveRouteCb().

◆ world_model_

carma_wm::WorldModelConstPtr route::RouteGeneratorWorker::world_model_

private

Definition at line 273 of file route_generator_worker.hpp.

Referenced by bumperPoseCb(), rerouteAfterRouteInvalidation(), setActiveRouteCb(), setWorldModelPtr(), and spinCallback().

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

route/include/route/route_generator_worker.hpp
route/src/route_generator_worker.cpp

Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Static Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ RouteGeneratorWorker()

Member Function Documentation

◆ abortActiveRouteCb()

◆ addLanelet()

◆ bumperPoseCb()

◆ checkForDuplicateLaneletsInShortestPath()

◆ composeRouteMarkerMsg()

◆ composeRouteMsg()

◆ crosstrackErrorCheck()

◆ georeferenceCb()

◆ getAvailableRouteCb()

◆ getClosestLaneletFromRouteLanelets()

◆ initializeBumperTransformLookup()

◆ loadRouteDestinationGpsPointsFromRouteId()

◆ loadRouteDestinationsInMapFrame()

◆ publishRouteEvent()

◆ rerouteAfterRouteInvalidation()

◆ routing()

◆ setActiveRouteCb()

◆ setClock()

◆ setCrosstrackErrorCountMax()

◆ setCrosstrackErrorDistance()

◆ setDowntrackDestinationRange()

◆ setLoggerInterface()

◆ setPublishers()

◆ setReroutingChecker()

◆ setRouteFilePath()

◆ setWorldModelPtr()

◆ spinCallback()

◆ twistCb()

Member Data Documentation

◆ clock_

◆ cross_track_dist_

◆ cte_count_

◆ cte_count_max_

◆ current_crosstrack_distance_

◆ current_downtrack_distance_

◆ current_loc_

◆ current_speed_

◆ DEG_TO_RAD

◆ destination_points_in_map_

◆ down_track_target_range_

◆ epsilon_

◆ frontbumper_transform_

◆ ll_crosstrack_distance_

◆ ll_downtrack_distance_

◆ ll_id_

◆ logger_

◆ map_proj_

◆ new_route_marker_generated_

◆ new_route_msg_generated_

◆ points_

◆ reroutingChecker

◆ route_event_msg_

◆ route_event_pub_

◆ route_event_queue

◆ route_file_path_

◆ route_llts

◆ route_marker_msg_

◆ route_marker_pub_

◆ route_msg_

◆ route_pub_

◆ route_state_msg_

◆ route_state_pub_

◆ rs_worker_

◆ speed_limit_

◆ tf2_buffer_

◆ tf2_listener_

◆ tf_

◆ vehicle_pose_

◆ world_model_