ROS node for the inlanecruising_plugin. More...

#include <intersection_transit_maneuvering_node.hpp>

Inheritance diagram for intersection_transit_maneuvering::IntersectionTransitManeuveringNode:

Collaboration diagram for intersection_transit_maneuvering::IntersectionTransitManeuveringNode:

Public Member Functions
	IntersectionTransitManeuveringNode (const rclcpp::NodeOptions &)
	IntersectionTransitManeuveringNode constructor. More...

std::vector< carma_planning_msgs::msg::Maneuver >	convert_maneuver_plan (const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers)
	Converts a sequence of INTERSECTION_TRANSIT maneuvers to LANE_FOLLOWING maneuvers. More...

void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...

bool	get_availability () override
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

std::string	get_version_id () override
	Returns the version id of this plugin. More...

carma_ros2_utils::CallbackReturn	on_configure_plugin () override
	This method should be used to load parameters and will be called on the configure state transition. More...

Public Member Functions inherited from carma_guidance_plugins::TacticalPlugin
	TacticalPlugin (const rclcpp::NodeOptions &)
	TacticalPlugin constructor. More...

virtual	~TacticalPlugin ()=default
	Virtual destructor for safe deletion. More...

virtual void	plan_trajectory_callback (std::shared_ptr< rmw_request_id_t > srv_header, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)=0
	Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request. More...

std::string	get_capability () override
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

uint8_t	get_type () override final
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override final

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override final

Public Member Functions inherited from carma_guidance_plugins::PluginBaseNode
	PluginBaseNode (const rclcpp::NodeOptions &)
	PluginBaseNode constructor. More...

virtual	~PluginBaseNode ()=default
	Virtual destructor for safe deletion. More...

virtual std::shared_ptr< carma_wm::WMListener >	get_world_model_listener () final
	Method to return the default world model listener provided as a convience by this base class If this method or get_world_model() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual carma_wm::WorldModelConstPtr	get_world_model () final
	Method to return the default world model provided as a convience by this base class If this method or get_world_model_listener() are not called then the world model remains uninitialized and will not create unnecessary subscriptions. More...

virtual bool	get_activation_status () final
	Returns the activation status of this plugin. The plugins API callbacks will only be triggered when this method returns true. More...

virtual uint8_t	get_type ()
	Returns the type of this plugin according to the carma_planning_msgs::Plugin type enum. Extending classes for the specific type should override this method. More...

std::string	get_plugin_name_and_ns () const
	Return the name of this plugin with namespace. NOTE: If only the name of the plugin is required, use get_plugin_name() More...

std::string	get_plugin_name () const
	Return the name of this plugin. More...

virtual bool	get_availability ()=0
	Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes. More...

virtual std::string	get_capability ()=0
	Get the capability string representing this plugins capabilities Method must be overriden by extending classes. Expectation is that abstract plugin type parent classes will provide a default implementation. More...

virtual std::string	get_version_id ()=0
	Returns the version id of this plugin. More...

virtual carma_ros2_utils::CallbackReturn	on_configure_plugin ()=0
	Method which is triggered when this plugin is moved from the UNCONFIGURED to INACTIVE states. This method should be used to load parameters and is required to be implemented. More...

virtual carma_ros2_utils::CallbackReturn	on_activate_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to ACTIVE states. This method should be used to prepare for future callbacks for plugin's capabilites. More...

virtual carma_ros2_utils::CallbackReturn	on_deactivate_plugin ()
	Method which is triggered when this plugin is moved from the ACTIVE to INACTIVE states. This method should be used to disable any functionality which should cease execution when plugin is inactive. More...

virtual carma_ros2_utils::CallbackReturn	on_cleanup_plugin ()
	Method which is triggered when this plugin is moved from the INACTIVE to UNCONFIGURED states. This method should be used to fully reset the plugin such that a future call to on_configure_plugin would leave the plugin in a fresh state as though just launched. More...

virtual carma_ros2_utils::CallbackReturn	on_shutdown_plugin ()
	Method which is triggered when this plugin is moved from any state to FINALIZED This method should be used to generate any shutdown logs or final cleanup. More...

virtual carma_ros2_utils::CallbackReturn	on_error_plugin (const std::string &exception_string)
	Method which is triggered when an unhandled exception occurs in this plugin This method should be used to cleanup such that the plugin could be moved to UNCONFIGURED state if possible. More...

carma_ros2_utils::CallbackReturn	handle_on_configure (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_activate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_deactivate (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_cleanup (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_shutdown (const rclcpp_lifecycle::State &) override

carma_ros2_utils::CallbackReturn	handle_on_error (const rclcpp_lifecycle::State &, const std::string &exception_string) override

	FRIEND_TEST (carma_guidance_plugins_test, connections_test)

Public Attributes
std::shared_ptr< CallInterface >	object_

carma_ros2_utils::ClientPtr< carma_planning_msgs::srv::PlanTrajectory >	client_

Private Attributes
std::vector< carma_planning_msgs::msg::Maneuver >	converted_maneuvers_

carma_planning_msgs::msg::VehicleState	vehicle_state_

Detailed Description

ROS node for the inlanecruising_plugin.

Definition at line 33 of file intersection_transit_maneuvering_node.hpp.

Constructor & Destructor Documentation

◆ IntersectionTransitManeuveringNode()

intersection_transit_maneuvering::IntersectionTransitManeuveringNode::IntersectionTransitManeuveringNode ( const rclcpp::NodeOptions & options )

explicit

IntersectionTransitManeuveringNode constructor.

Definition at line 133 of file intersection_transit_maneuvering.cpp.

134: carma_guidance_plugins::TacticalPlugin(options) {}

carma_guidance_plugins::TacticalPlugin

TacticalPlugin base class which can be extended by user provided plugins which wish to implement the ...

Definition: tactical_plugin.hpp:34

Member Function Documentation

◆ convert_maneuver_plan()

std::vector< carma_planning_msgs::msg::Maneuver > intersection_transit_maneuvering::IntersectionTransitManeuveringNode::convert_maneuver_plan ( const std::vector< carma_planning_msgs::msg::Maneuver > & maneuvers )

Converts a sequence of INTERSECTION_TRANSIT maneuvers to LANE_FOLLOWING maneuvers.

Parameters

maneuvers The list of maneuvers to convert

Returns: The new list of converted maneuvers

Definition at line 228 of file intersection_transit_maneuvering.cpp.

    {
       if (maneuvers.size() == 0)
        {
            throw std::invalid_argument("No maneuvers to convert");
        }
 
        std::vector<carma_planning_msgs::msg::Maneuver> new_maneuver_plan;
        carma_planning_msgs::msg::Maneuver new_maneuver;
        new_maneuver.type = carma_planning_msgs::msg::Maneuver::LANE_FOLLOWING; //All of the converted maneuvers will be of type LANE_FOLLOWING
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Input Maneuver Type = "<< static_cast<int>(maneuvers.front().type));
        for(const auto& maneuver : maneuvers)
        {
            //Throw exception if the manuever type does not match INTERSECTION_TRANSIT
            if ( maneuver.type != carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT &&
                maneuver.type != carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN &&
                maneuver.type != carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN)
                {
                    throw std::invalid_argument("Intersection transit maneuvering does not support this maneuver type");
                }
 
            //Convert IT Straight
            if (maneuver.type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Converting INTERSECTION_TRANSIT_STRAIGHT");
 
                new_maneuver.lane_following_maneuver.parameters.maneuver_id = maneuver.intersection_transit_straight_maneuver.parameters.maneuver_id;
                new_maneuver.lane_following_maneuver.parameters.planning_strategic_plugin = maneuver.intersection_transit_straight_maneuver.parameters.planning_strategic_plugin;
                new_maneuver.lane_following_maneuver.parameters.planning_tactical_plugin = maneuver.intersection_transit_straight_maneuver.parameters.planning_tactical_plugin;
                new_maneuver.lane_following_maneuver.parameters.negotiation_type = maneuver.intersection_transit_straight_maneuver.parameters.negotiation_type;
                new_maneuver.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
 
                new_maneuver.lane_following_maneuver.start_dist = maneuver.intersection_transit_straight_maneuver.start_dist;
                new_maneuver.lane_following_maneuver.start_speed = maneuver.intersection_transit_straight_maneuver.start_speed;
                new_maneuver.lane_following_maneuver.start_time = maneuver.intersection_transit_straight_maneuver.start_time;
 
                new_maneuver.lane_following_maneuver.end_dist = maneuver.intersection_transit_straight_maneuver.end_dist;
                new_maneuver.lane_following_maneuver.end_speed = maneuver.intersection_transit_straight_maneuver.end_speed;
                new_maneuver.lane_following_maneuver.end_time = maneuver.intersection_transit_straight_maneuver.end_time;
 
 
                new_maneuver.lane_following_maneuver.lane_ids = { maneuver.intersection_transit_straight_maneuver.starting_lane_id };
 
                new_maneuver_plan.push_back(new_maneuver);
            }
 
             //Convert IT LEFT TURN
            if (maneuver.type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN)
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Converting INTERSECTION_TRANSIT_LEFT_TURN");
 
                new_maneuver.lane_following_maneuver.parameters.maneuver_id = maneuver.intersection_transit_left_turn_maneuver.parameters.maneuver_id;
                new_maneuver.lane_following_maneuver.parameters.planning_strategic_plugin = maneuver.intersection_transit_left_turn_maneuver.parameters.planning_strategic_plugin;
                new_maneuver.lane_following_maneuver.parameters.planning_tactical_plugin = maneuver.intersection_transit_left_turn_maneuver.parameters.planning_tactical_plugin;
                new_maneuver.lane_following_maneuver.parameters.negotiation_type = maneuver.intersection_transit_left_turn_maneuver.parameters.negotiation_type;
                new_maneuver.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
 
                new_maneuver.lane_following_maneuver.start_dist = maneuver.intersection_transit_left_turn_maneuver.start_dist;
                new_maneuver.lane_following_maneuver.start_speed = maneuver.intersection_transit_left_turn_maneuver.start_speed;
                new_maneuver.lane_following_maneuver.start_time = maneuver.intersection_transit_left_turn_maneuver.start_time;
 
                new_maneuver.lane_following_maneuver.end_dist = maneuver.intersection_transit_left_turn_maneuver.end_dist;
                new_maneuver.lane_following_maneuver.end_speed = maneuver.intersection_transit_left_turn_maneuver.end_speed;
                new_maneuver.lane_following_maneuver.end_time = maneuver.intersection_transit_left_turn_maneuver.end_time;
 
                new_maneuver.lane_following_maneuver.lane_ids = { maneuver.intersection_transit_left_turn_maneuver.starting_lane_id };
 
                new_maneuver_plan.push_back(new_maneuver);
            }
 
             //Convert IT RIGHT TURN
            if (maneuver.type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN)
            {
 
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Converting INTERSECTION_TRANSIT_RIGHT_TURN");
 
                new_maneuver.lane_following_maneuver.parameters.maneuver_id = maneuver.intersection_transit_right_turn_maneuver.parameters.maneuver_id;
                new_maneuver.lane_following_maneuver.parameters.planning_strategic_plugin = maneuver.intersection_transit_right_turn_maneuver.parameters.planning_strategic_plugin;
                new_maneuver.lane_following_maneuver.parameters.planning_tactical_plugin = maneuver.intersection_transit_right_turn_maneuver.parameters.planning_tactical_plugin;
                new_maneuver.lane_following_maneuver.parameters.negotiation_type = maneuver.intersection_transit_right_turn_maneuver.parameters.negotiation_type;
                new_maneuver.lane_following_maneuver.parameters.presence_vector = carma_planning_msgs::msg::ManeuverParameters::HAS_TACTICAL_PLUGIN;
 
                new_maneuver.lane_following_maneuver.start_dist = maneuver.intersection_transit_right_turn_maneuver.start_dist;
                new_maneuver.lane_following_maneuver.start_speed = maneuver.intersection_transit_right_turn_maneuver.start_speed;
                new_maneuver.lane_following_maneuver.start_time = maneuver.intersection_transit_right_turn_maneuver.start_time;
 
                new_maneuver.lane_following_maneuver.end_dist = maneuver.intersection_transit_right_turn_maneuver.end_dist;
                new_maneuver.lane_following_maneuver.end_speed = maneuver.intersection_transit_right_turn_maneuver.end_speed;
                new_maneuver.lane_following_maneuver.end_time = maneuver.intersection_transit_right_turn_maneuver.end_time;
 
                new_maneuver.lane_following_maneuver.lane_ids = { maneuver.intersection_transit_right_turn_maneuver.starting_lane_id };
 
                new_maneuver_plan.push_back(new_maneuver);
            }
 
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Original Maneuver : " << maneuver << std::endl <<   "Converted Maneuver: " << new_maneuver);
  
        }//end for-loop 
 
        return new_maneuver_plan; 
 
    }

Referenced by plan_trajectory_callback().

Here is the caller graph for this function:

◆ get_availability()

bool intersection_transit_maneuvering::IntersectionTransitManeuveringNode::get_availability ( )

overridevirtual

Get the availability status of this plugin based on the current operating environment. Method must be overriden by extending classes.

Returns: This method should return true if the plugin's current understanding of the world means it would be capable of planning or executing its capability.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 151 of file intersection_transit_maneuvering.cpp.

                                                            {
    return true;
  }

◆ get_version_id()

std::string intersection_transit_maneuvering::IntersectionTransitManeuveringNode::get_version_id ( )

overridevirtual

Returns the version id of this plugin.

Returns: The version id represented as a string

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 155 of file intersection_transit_maneuvering.cpp.

                                                               {
    return "v4.0"; // Version ID matches the value set in this package's package.xml
  }   

◆ on_configure_plugin()

carma_ros2_utils::CallbackReturn intersection_transit_maneuvering::IntersectionTransitManeuveringNode::on_configure_plugin ( )

overridevirtual

This method should be used to load parameters and will be called on the configure state transition.

Implements carma_guidance_plugins::PluginBaseNode.

Definition at line 136 of file intersection_transit_maneuvering.cpp.

{
 RCLCPP_INFO_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"), "IntersectionTransitManeuveringNode trying to configure");
                        
 object_ = std::make_shared<intersection_transit_maneuvering::Servicer>();
 
 client_  = create_client<carma_planning_msgs::srv::PlanTrajectory>("inlanecruising_plugin/plan_trajectory");
 
 object_->set_client(client_);
 
 //Return success if everthing initialized successfully
return CallbackReturn::SUCCESS;
 
}

References client_, and object_.

◆ plan_trajectory_callback()

void intersection_transit_maneuvering::IntersectionTransitManeuveringNode::plan_trajectory_callback	(	std::shared_ptr< rmw_request_id_t >	srv_header,
		carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr	req,
		carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr	resp
	)

overridevirtual

Extending class provided callback which should return a planned trajectory based on the provided trajectory planning request.

Parameters

srv_header	RCL header for services calls. Can usually be ignored by implementers.
req	The service request containing the maneuvers to plan trajectories for and current vehicle state
resp	The response containing the planned trajectory

Implements carma_guidance_plugins::TacticalPlugin.

Definition at line 160 of file intersection_transit_maneuvering.cpp.

      {
 
        std::chrono::system_clock::time_point start_time = std::chrono::system_clock::now();  // Start timing the execution time for planning so it can be logged
 
        std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
 
        auto related_maneuvers = resp->related_maneuvers;
 
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Starting planning for maneuver index: " << req->maneuver_index_to_plan);
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"req->maneuver_plan.maneuvers.size(): " << req->maneuver_plan.maneuvers.size());
 
        for(size_t i = req->maneuver_index_to_plan; i < req->maneuver_plan.maneuvers.size(); i++)
        {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Looping");
            if(req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_STRAIGHT ||
            req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_LEFT_TURN || 
            req->maneuver_plan.maneuvers[i].type == carma_planning_msgs::msg::Maneuver::INTERSECTION_TRANSIT_RIGHT_TURN )
            {
                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Found valid maneuver for planning at index: " << i);
                maneuver_plan.push_back(req->maneuver_plan.maneuvers[i]);
                related_maneuvers.push_back(i);
            }
            else
            {
                break;
            }
        }
 
        converted_maneuvers_ = convert_maneuver_plan(maneuver_plan);
        auto new_req = std::make_shared<carma_planning_msgs::srv::PlanTrajectory::Request> ();
 
        for(auto i : converted_maneuvers_)
        {
            new_req->maneuver_plan.maneuvers.push_back(i);
        }
 
        new_req->header = req->header;
        new_req->vehicle_state = req->vehicle_state;
        new_req->initial_trajectory_plan = req->initial_trajectory_plan;
        carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr ilc_resp = std::make_shared<carma_planning_msgs::srv::PlanTrajectory::Response>();
        object_->call(new_req,ilc_resp);
 
        if(ilc_resp->trajectory_plan.trajectory_points.empty())//Since we're using an interface for this process, the call() functionality will come from somewhere else
        {
           RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Failed to call service");
           return ;
        } else {
            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"Call Successful");
            resp->trajectory_plan = ilc_resp->trajectory_plan;
        }
 
 
        resp->related_maneuvers = related_maneuvers; // Set the related maneuvers using the origional maneuver indexs not those sent to inlane-cruising
        for (auto maneuver : related_maneuvers) {
            resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
        }
 
        std::chrono::system_clock::time_point end_time = std::chrono::system_clock::now();  // Start timing the execution time for planning so it can be logged
 
        auto duration = end_time - start_time;
        RCLCPP_DEBUG_STREAM(rclcpp::get_logger("intersection_transit_maneuvering"),"ExecutionTime: " <<  std::chrono::duration<double>(duration).count());
        return ; 
    }