plan_delegator.hpp

Go to the documentation of this file.

#pragma once
/*
 * Copyright (C) 2022-2023 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 <unordered_map>
#include <math.h>
#include <rclcpp/rclcpp.hpp>
#include <gtest/gtest_prod.h>
#include <carma_planning_msgs/msg/maneuver_plan.hpp>
#include <carma_planning_msgs/msg/guidance_state.hpp>
#include <carma_planning_msgs/msg/upcoming_lane_change_status.hpp>
#include <carma_planning_msgs/srv/plan_trajectory.hpp>
#include <carma_ros2_utils/carma_lifecycle_node.hpp>
#include <basic_autonomy/basic_autonomy.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <autoware_msgs/msg/lamp_cmd.hpp>
#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 <carma_wm/WMListener.hpp>
#include <carma_wm/WorldModel.hpp>
#include <carma_wm/Geometry.hpp>
#include <string>
 
// TODO Replace this Macro if possible
#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 new std::invalid_argument("GET_MANEUVER_PROPERTY (property) called on maneuver with invalid type id"))))))))
 
 
#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("ADJUST_MANEUVER_PROPERTY (property) called on maneuver with invalid type id " + std::to_string((mvr).type)))))))))
 
 
namespace plan_delegator
{
    struct Config
    {
        // ROS params
        std::string planning_topic_prefix = "/plugins/";
        std::string planning_topic_suffix = "/plan_trajectory";
        double trajectory_planning_rate = 10.0;
        double max_trajectory_duration = 6.0;
        double min_crawl_speed = 2.2352; // Min crawl speed in m/s
        double duration_to_signal_before_lane_change = 2.5; // (Seconds) If an upcoming lane change will begin in under this time threshold, a turn signal activation command will be published.
        int tactical_plugin_service_call_timeout = 100; // (Milliseconds) The maximum duration that Plan Delegator will wait after calling a tactical plugin's trajectory planning service; if trajectory
                                                        // generation takes longer than this, then planning will immediately end for the current trajectory planning iteration.
        int max_traj_generation_reattempt = 5; // The maximum number of times plan_delegator attempts to generate a trajectory before giving up
        bool enable_object_avoidance = true; // Activate object avoidance logic by calling yield_plugin on the final trajectory before publishing
 
        // Stream operator for this config
        friend std::ostream &operator<<(std::ostream &output, const Config &c)
        {
        output << "PlanDelegator::Config { " << std::endl
            << "planning_topic_prefix: " << c.planning_topic_prefix << std::endl
            << "planning_topic_suffix: " << c.planning_topic_suffix << std::endl
            << "trajectory_planning_rate: " << c.trajectory_planning_rate << std::endl
            << "max_trajectory_duration: " << c.max_trajectory_duration << std::endl
            << "min_crawl_speed: " << c.min_crawl_speed << std::endl
            << "tactical_plugin_service_call_timeout: " << c.tactical_plugin_service_call_timeout << std::endl
            << "max_traj_generation_reattempt: " << c.max_traj_generation_reattempt << std::endl
            << "duration_to_signal_before_lane_change: " << c.duration_to_signal_before_lane_change << std::endl
            << "enable_object_avoidance: " << c.enable_object_avoidance << std::endl
            << "}" << std::endl;
        return output;
        }
    };
 
    struct LaneChangeInformation
    {
        double starting_downtrack;  // The starting downtrack of the lane change
        bool is_right_lane_change;  // Flag to indicate whether lane change is a right lane change; false if it is a left lane change
    };
 
    class PlanDelegator : public carma_ros2_utils::CarmaLifecycleNode
    {
        public:
 
            // constants definition
            static const constexpr double MILLISECOND_TO_SECOND = 0.001;
 
            explicit PlanDelegator(const rclcpp::NodeOptions &);
 
            void maneuverPlanCallback(carma_planning_msgs::msg::ManeuverPlan::UniquePtr plan);
 
            void guidanceStateCallback(carma_planning_msgs::msg::GuidanceState::UniquePtr plan);
 
            void poseCallback(geometry_msgs::msg::PoseStamped::UniquePtr pose_msg);
 
            carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> getPlannerClientByName(const std::string& planner_name);
 
            bool isManeuverExpired(const carma_planning_msgs::msg::Maneuver& maneuver, rclcpp::Time current_time) const;
 
            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;
 
            void lookupFrontBumperTransform();
 
            void updateManeuverParameters(carma_planning_msgs::msg::Maneuver& maneuver);
 
            // Overrides
            carma_ros2_utils::CallbackReturn handle_on_configure(const rclcpp_lifecycle::State &);
            carma_ros2_utils::CallbackReturn handle_on_activate(const rclcpp_lifecycle::State &);
 
        protected:
            // Node configuration
            Config config_;
 
            // map to store service clients
            std::unordered_map<std::string, carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory>> trajectory_planners_;
            // service client for yield_plugin, used to adjust the final trajectory before publishing to avoid obstacles
            carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanTrajectory> yield_client_;
            // local storage of incoming messages
            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_;
 
            // wm listener pointer and pointer to the actual wm object
            carma_wm::WMListener wml_;
            carma_wm::WorldModelConstPtr wm_;
 
        private:
            // ROS Publishers
            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_;
 
            // ROS Subscribers
            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_;
            // Separate callback group for the timer to avoid blocking other callbacks like pose
            rclcpp::CallbackGroup::SharedPtr timer_callback_group_;
 
            bool guidance_engaged = false;
            int consecutive_traj_gen_failure_num_ = 0; // Number of consecutive trajectory generation failures
 
            double length_to_front_bumper_ = 3.0;
 
            // TF listenser
            std::shared_ptr<tf2_ros::Buffer> tf2_buffer_;
            std::shared_ptr<tf2_ros::TransformListener> tf2_listener_;
 
            // Object to store information regarding the next upcoming lane change in latest_maneuver_plan_; empty if no upcoming lane change exists in latest_maneuver_plan_
            boost::optional<LaneChangeInformation> upcoming_lane_change_information_;
 
            // Object to store information regarding the current active lane change in latest_maneuver_plan_; empty if first maneuver in latest_maneuver_plan_ is not a lane change
            boost::optional<LaneChangeInformation> current_lane_change_information_;
 
            // The latest UpcomingLaneChangeStatus that was published to upcoming_lane_change_status_pub_.
            carma_planning_msgs::msg::UpcomingLaneChangeStatus upcoming_lane_change_status_;
 
            // The latest turn signal command published to turn_signal_command_pub_.
            autoware_msgs::msg::LampCmd latest_turn_signal_command_;
 
            void onTrajPlanTick();
 
            bool isManeuverPlanValid(const carma_planning_msgs::msg::ManeuverPlan& maneuver_plan) const noexcept;
 
            bool isTrajectoryValid(const carma_planning_msgs::msg::TrajectoryPlan& trajectory_plan) const noexcept;
 
            bool isTrajectoryLongEnough(const carma_planning_msgs::msg::TrajectoryPlan& plan) const noexcept;
 
            carma_planning_msgs::msg::TrajectoryPlan planTrajectory();
 
            LaneChangeInformation getLaneChangeInformation(const carma_planning_msgs::msg::Maneuver& lane_change_maneuver);
 
            void publishUpcomingLaneChangeStatus(const boost::optional<LaneChangeInformation>& upcoming_lane_change_information);
 
            void publishTurnSignalCommand(const boost::optional<LaneChangeInformation>& current_lane_change_information, const carma_planning_msgs::msg::UpcomingLaneChangeStatus& upcoming_lane_change_status);
 
            // Unit Test Accessors
            FRIEND_TEST(TestPlanDelegator, UnitTestPlanDelegator);
            FRIEND_TEST(TestPlanDelegator, TestPlanDelegator);
            FRIEND_TEST(TestPlanDelegator, TestLaneChangeInformation);
            FRIEND_TEST(TestPlanDelegator, TestUpcomingLaneChangeAndTurnSignals);
            FRIEND_TEST(TestPlanDelegator, TestUpdateManeuverParameters);
            FRIEND_TEST(TestPlanDelegator, TestGetLaneChangeInformation);
    };
}