route_following_plugin.hpp

Go to the documentation of this file.

#pragma once
 
/*
 * Copyright (C) 2019-2021 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 <vector>
#include <carma_planning_msgs/msg/plugin.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
#include <carma_planning_msgs/msg/trajectory_plan.hpp>
#include <gtest/gtest_prod.h>
#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 <unordered_set>
#include "carma_guidance_plugins/strategic_plugin.hpp"
#include "route_following_plugin_config.hpp"
 
#define GET_MANEUVER_PROPERTY(mvr, property)\
        (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property :\
            ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property :\
                ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property :\
                    ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property :\
                        ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property :\
                            ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property :\
                                throw std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))))
 
#define SET_MANEUVER_PROPERTY(mvr, property, value)\
        (((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN ? (mvr).intersection_transit_left_turn_maneuver.property = (value) :\
            ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN ? (mvr).intersection_transit_right_turn_maneuver.property = (value) :\
                ((mvr).type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ? (mvr).intersection_transit_straight_maneuver.property = (value) :\
                    ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_CHANGE ? (mvr).lane_change_maneuver.property = (value) :\
                        ((mvr).type == carma_planning_msgs::msg::Maneuver::STOP_AND_WAIT ? (mvr).stop_and_wait_maneuver.property = (value) :\
                            ((mvr).type == carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING ? (mvr).lane_following_maneuver.property = (value) :\
                                throw std::invalid_argument("SET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))))
                        
namespace route_following_plugin
{
    class RouteFollowingPlugin : public carma_guidance_plugins::StrategicPlugin
    {
        public:
 
        explicit RouteFollowingPlugin(const rclcpp::NodeOptions &);
        
        carma_ros2_utils::CallbackReturn on_configure_plugin();
        carma_ros2_utils::CallbackReturn on_activate_plugin();
 
        // wm listener pointer and pointer to the actual wm object
        std::shared_ptr<carma_wm::WMListener> wml_;
        carma_wm::WorldModelConstPtr wm_;
 
        bool get_availability();
        std::string get_version_id();
        
        private:
 
        carma_planning_msgs::msg::Maneuver composeLaneFollowingManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, const std::vector<lanelet::Id>& lane_ids) const;
 
        carma_planning_msgs::msg::Maneuver composeLaneChangeManeuverMessage(double start_dist, double end_dist, double start_speed, double target_speed, lanelet::Id starting_lane_id,lanelet::Id ending_lane_id) const;
        
        carma_planning_msgs::msg::Maneuver composeStopAndWaitManeuverMessage(double start_dist, double end_dist, double start_speed, lanelet::Id starting_lane_id, lanelet::Id ending_lane_id, double stopping_accel) const;
 
        bool isLaneChangeNeeded(lanelet::routing::LaneletRelations relations, lanelet::Id target_id) const;
        
        void setManeuverStartDist(carma_planning_msgs::msg::Maneuver& maneuver, double start_dist) const;
 
        void updateTimeProgress(std::vector<carma_planning_msgs::msg::Maneuver>& maneuvers, rclcpp::Time start_time) const;
        void updateStartingSpeed(carma_planning_msgs::msg::Maneuver& maneuver, double start_speed) const;
 
        void plan_maneuvers_callback(
         std::shared_ptr<rmw_request_id_t> srv_header, 
         carma_planning_msgs::srv::PlanManeuvers::Request::SharedPtr req, 
         carma_planning_msgs::srv::PlanManeuvers::Response::SharedPtr resp);
 
        double findSpeedLimit(const lanelet::ConstLanelet& llt);
 
        std::vector<carma_planning_msgs::msg::Maneuver> routeCb(const lanelet::routing::LaneletPath& route_shortest_path);
 
        std::vector<carma_planning_msgs::msg::Maneuver> addStopAndWaitAtRouteEnd (
                const std::vector<carma_planning_msgs::msg::Maneuver>& input_maneuvers, 
                double route_end_downtrack, double stopping_entry_speed, double stopping_logitudinal_accel,
                double lateral_accel_limit, double min_maneuver_length
            ) const;
 
        bool maneuverWithBufferStartsAfterDowntrack(const carma_planning_msgs::msg::Maneuver& maneuver, double downtrack, double lateral_accel, double min_maneuver_length) const;
 
        std::string getNewManeuverId() const;
 
        void returnToShortestPath(const lanelet::ConstLanelet &current_lanelet);
 
        //Subscribers
        carma_ros2_utils::SubPtr<geometry_msgs::msg::TwistStamped> twist_sub_;
        carma_ros2_utils::SubPtr<carma_planning_msgs::msg::ManeuverPlan> current_maneuver_plan_sub_;
    
        // unordered set of all the lanelet ids in shortest path
        std::unordered_set<lanelet::Id> shortest_path_set_;
       
        static constexpr double MAX_LANE_WIDTH = 3.70; // Maximum lane width of a US highway
 
        // Node configuration
        Config config_;
        
        // Current vehicle forward speed
        double current_speed_ = 0.0;
 
        //Small constant to compare doubles against
        double epsilon_ = 0.0001;
 
        // Current vehicle pose in map
        geometry_msgs::msg::PoseStamped pose_msg_;
        lanelet::BasicPoint2d current_loc_;
        
        // Currently executing maneuver plan from Arbitrator
        carma_planning_msgs::msg::ManeuverPlan::UniquePtr current_maneuver_plan_;
 
        //Queue of maneuver plans
        std::vector<carma_planning_msgs::msg::Maneuver> latest_maneuver_plan_;
 
        //Tactical plugin being used for planning lane change
        std::string lane_change_plugin_ = "cooperative_lanechange";
        std::string stop_and_wait_plugin_ = "stop_and_wait_plugin";
 
        std::string planning_strategic_plugin_ = "route_following_plugin";
        std::string lanefollow_planning_tactical_plugin_ = "inlanecruising_plugin"; 
   
        // Timer used to update the front bumper pose
        rclcpp::TimerBase::SharedPtr bumper_pose_timer_;
        
        void bumper_pose_cb();
 
        void twist_cb(geometry_msgs::msg::TwistStamped::UniquePtr msg);
 
        void current_maneuver_plan_cb(carma_planning_msgs::msg::ManeuverPlan::UniquePtr msg);
 
        rclcpp::Duration getManeuverDuration(carma_planning_msgs::msg::Maneuver &maneuver, double epsilon) const;
 
        void initializeBumperTransformLookup();
 
        geometry_msgs::msg::TransformStamped tf_;
        
        // front bumper transform
        tf2::Stamped<tf2::Transform> frontbumper_transform_;
        
        // TF listenser
        tf2_ros::Buffer tf2_buffer_;
        std::unique_ptr<tf2_ros::TransformListener> tf2_listener_;
 
        //Unit Tests
        FRIEND_TEST(RouteFollowingPluginTest, testComposeManeuverMessage);
        FRIEND_TEST(RouteFollowingPluginTest, testIdentifyLaneChange);
        FRIEND_TEST(RouteFollowingPlugin, TestAssociateSpeedLimit);
        FRIEND_TEST(RouteFollowingPlugin, TestAssociateSpeedLimitusingosm);
        FRIEND_TEST(RouteFollowingPlugin, TestHelperfunctions);
        FRIEND_TEST(RouteFollowingPlugin, TestReturnToShortestPath);
        FRIEND_TEST(StopAndWaitTestFixture, CaseOne);
        FRIEND_TEST(StopAndWaitTestFixture, CaseTwo);
        FRIEND_TEST(StopAndWaitTestFixture, CaseThree);
        FRIEND_TEST(StopAndWaitTestFixture, CaseFour);
        FRIEND_TEST(StopAndWaitTestFixture, CaseFive);
        FRIEND_TEST(StopAndWaitTestFixture, CaseSix);
        FRIEND_TEST(StopAndWaitTestFixture, CaseSeven);
        FRIEND_TEST(StopAndWaitTestFixture, CaseEight);
        FRIEND_TEST(StopAndWaitTestFixture, CaseNine);
        FRIEND_TEST(StopAndWaitTestFixture, CaseTen);
 
    };
}