route_generator_worker.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2020-2022 LEIDOS.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
 * use this file except in compliance with the License. You may obtain a copy of
 * the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
 * License for the specific language governing permissions and limitations under
 * the License.
 */
 
#include <rclcpp/rclcpp.hpp>
 
#include <carma_planning_msgs/msg/route.hpp>
#include <carma_planning_msgs/msg/route_state.hpp>
#include <carma_planning_msgs/msg/route_event.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <std_msgs/msg/string.hpp>
#include <carma_planning_msgs/srv/get_available_routes.hpp>
#include <carma_planning_msgs/srv/set_active_route.hpp>
#include <carma_planning_msgs/srv/abort_active_route.hpp>
 
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_wm/Geometry.hpp>
 
#include <lanelet2_core/geometry/Lanelet.h>
#include <lanelet2_core/primitives/Lanelet.h>
#include <lanelet2_extension/projection/local_frame_projector.h>
#include <lanelet2_extension/io/autoware_osm_parser.h>
#include <lanelet2_core/utility/Utilities.h>
 
#include <wgs84_utils/wgs84_utils.h>
#include <boost/filesystem.hpp>
#include <boost/optional.hpp>
#include <math.h>
#include <unordered_set>
#include <functional>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
#include "route/route_state_worker.hpp"
 
namespace route {
 
    class RouteGeneratorWorker
    {
 
    public:
        RouteGeneratorWorker(tf2_ros::Buffer& tf2_buffer);
 
        std::function<bool()> reroutingChecker;
 
        void setReroutingChecker(const std::function<bool()> inputFunction);
        
        void setWorldModelPtr(carma_wm::WorldModelConstPtr wm);
 
        void setLoggerInterface(rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger);
 
        void setClock(rclcpp::Clock::SharedPtr clock);
 
        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;
 
        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);
        
        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);
 
        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);
 
        void bumperPoseCb();
 
        void twistCb(geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
        void georeferenceCb(std_msgs::msg::String::UniquePtr msg);
 
        void setRouteFilePath(const std::string& path);
 
        void setDowntrackDestinationRange(double dt_dest_range);
 
        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);
        
        bool checkForDuplicateLaneletsInShortestPath(const lanelet::routing::Route& route) const;
 
        std::vector<lanelet::BasicPoint3d> loadRouteDestinationsInMapFrame(const std::vector<carma_v2x_msgs::msg::Position3D>& destinations) const;
 
        std::vector<carma_v2x_msgs::msg::Position3D> loadRouteDestinationGpsPointsFromRouteId(const std::string& route_id) const;
 
        carma_planning_msgs::msg::Route composeRouteMsg(const lanelet::Optional<lanelet::routing::Route>& route) const;
 
        bool spinCallback();
 
        visualization_msgs::msg::Marker composeRouteMarkerMsg(const lanelet::Optional<lanelet::routing::Route>& route);
 
        bool crosstrackErrorCheck(const std::shared_ptr<geometry_msgs::msg::PoseStamped>& msg, lanelet::ConstLanelet current_llt);
 
        void setCrosstrackErrorCountMax(int cte_max);
 
        void setCrosstrackErrorDistance(double cte_dist);
 
        lanelet::ConstLanelet getClosestLaneletFromRouteLanelets(lanelet::BasicPoint2d position) const;
 
        // Added for Unit Testing
        void addLanelet(lanelet::ConstLanelet llt);
 
        lanelet::Optional<lanelet::routing::Route> rerouteAfterRouteInvalidation(const std::vector<lanelet::BasicPoint2d>& destination_points_in_map);
 
        void initializeBumperTransformLookup();
 
        // Current vehicle pose if it has been recieved
        boost::optional<geometry_msgs::msg::PoseStamped> vehicle_pose_;
 
    private:
 
        const double DEG_TO_RAD = 0.0174533;
        
        // route state worker
        RouteStateWorker rs_worker_;
 
        // directory of route files
        std::string route_file_path_;
 
        // const pointer to world model object
        carma_wm::WorldModelConstPtr world_model_;
 
        // route messages waiting to be updated and published
        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_; 
        
        //List of lanelets in the route
        lanelet::ConstLanelets route_llts;
        
        // minimum down track error which can trigger route complete event
        double down_track_target_range_;
 
        // current cross track distance
        double current_crosstrack_distance_;
 
        // current down track distance relevant to the route
        double current_downtrack_distance_;
 
        // current pose
        lanelet::BasicPoint2d current_loc_;
 
        // current lanelet cross track distance
        double ll_crosstrack_distance_;
 
        // current lanelet down track
        double ll_downtrack_distance_;
 
        lanelet::Id ll_id_;
 
        // current speed limit on current lanelet
        double speed_limit_ = 0;
        // Current vehicle forward speed
        double current_speed_ = 0;
        //A small static value for comparing doubles
        static constexpr double epsilon_ = 0.001;
 
        // local copy of Route publihsers
        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_;
 
        // a bool flag indicates a new route has been generated such that a local copy of Route message should be published again
        bool new_route_msg_generated_ = false;
        bool new_route_marker_generated_ = false;
 
        // a queue of route event. All events in the queue will be published in order on each spin.
        std::queue<uint8_t> route_event_queue;
 
        // private helper function to add a new route event into event queue
        void publishRouteEvent(uint8_t event_type);        
 
        // maximum cross track error which can trigger left route event
        double cross_track_dist_;
 
        // counter to record how many times vehicle's position exceeds crosstrack distance
        int cte_count_ = 0;
 
        int cte_count_max_;
 
        // destination points in map
        std::vector<lanelet::BasicPoint2d> destination_points_in_map_;
 
        // The current map projection for lat/lon to map frame conversion
        boost::optional<std::string> map_proj_;
 
        geometry_msgs::msg::TransformStamped tf_;
        tf2::Stamped<tf2::Transform> frontbumper_transform_;
 
        // TF listenser
        tf2_ros::Buffer& tf2_buffer_;
        std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
 
        // Logger interface
        rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger_;
 
        rclcpp::Clock::SharedPtr clock_;
    };
 
} // namespace route