#include <port_drayage_worker.hpp>

Collaboration diagram for port_drayage_plugin::PortDrayageWorker:

Public Member Functions
	PortDrayageWorker (rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr logger, rclcpp::Clock::SharedPtr clock, std::function< void(carma_v2x_msgs::msg::MobilityOperation)> mobility_operations_publisher, std::function< void(carma_msgs::msg::UIInstructions)> ui_instructions_publisher, std::function< bool(std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request >)> call_set_active_route)
	Standard constructor for the PortDrayageWorker. More...

void	setVehicleID (const std::string &cmv_id)
	Setter function to set this object's cmv_id_ string. More...

void	setCargoID (const std::string &cargo_id)
	Setter function to set this object's cargo_id_ string. More...

void	setEnablePortDrayageFlag (bool enable_port_drayage)
	Setter function to set this object's enable_port_drayage flag. More...

void	setStartingAtStagingAreaFlag (bool starting_at_staging_area)
	Setter function to set this object's starting_at_staging_area flag. More...

void	onArrivedAtDestination ()
	Callback for usage by the PortDrayageStateMachine when the vehicle has arrived at a destination. More...

void	onReceivedNewDestination ()
	Callback for usage by the PortDrayageStateMachine when the vehicle has received a new destination. More...

std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request >	composeSetActiveRouteRequest (boost::optional< double > dest_latitude, boost::optional< double > dest_longitude) const
	Create a SetActiveRoute service request to set a new active route for the system based on the destination points contained in the most recently-received Port Drayage MobilityOperation message intended for this vehicle. More...

carma_msgs::msg::UIInstructions	composeUIInstructions (const std::string &current_operation, const std::string &previous_operation)
	Creates a UIInstructions message that can be used to create a pop-up on the Web UI to notify a user that a new route has been received for a specified destination type, and that the system can be engaged on that route. More...

carma_v2x_msgs::msg::MobilityOperation	composeArrivalMessage () const
	Assemble the current dataset into a MobilityOperations message with a JSON formatted body containing CMV ID and cargo ID. More...

void	onNewGeoreference (std_msgs::msg::String::UniquePtr msg)
	Callback for map projection string to define lat/lon <--> map conversion. More...

void	onNewPose (geometry_msgs::msg::PoseStamped::UniquePtr msg)
	Callback for the pose subscriber. The pose will be converted into lat/lon and stored locally. More...

void	onInboundMobilityOperation (carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg)
	Callback to process a received MobilityOperation message. More...

void	updateCargoInformationAfterActionCompletion (const PortDrayageMobilityOperationMsg &previous_port_drayage_msg)
	Method to update worker's cargo-related data members depending on whether the previously completed action was for a pickup or dropoff. More...

void	mobilityOperationMessageParser (std::string mobility_operation_strategy_params)
	Function to help parse the text included in an inbound MobilityOperation message's strategy_params field according to the JSON schema intended for MobilityOperation messages with strategy type 'carma/port_drayage'. Stores the parsed information in _latest_mobility_operation_msg. More...

void	onGuidanceState (const carma_planning_msgs::msg::GuidanceState::UniquePtr msg)
	Callback to process the current status of the guidance state machine. More...

void	onRouteEvent (const carma_planning_msgs::msg::RouteEvent::UniquePtr msg)
	Callback to process each Route Event. More...

PortDrayageState	getPortDrayageState ()
	Get the current state of the port drayage state machine. More...

Public Attributes
PortDrayageMobilityOperationMsg	latest_mobility_operation_msg_

LatLonCoordinate	current_gps_position_

Private Attributes
std::shared_ptr< carma_planning_msgs::msg::RouteEvent >	latest_route_event_ = nullptr

PortDrayageStateMachine	pdsm_

std::string	previously_completed_operation_

std::string	cmv_id_

std::string	cargo_id_

std::function< void(carma_v2x_msgs::msg::MobilityOperation)>	publish_mobility_operation_

std::function< void(carma_msgs::msg::UIInstructions)>	publish_ui_instructions_

std::function< bool(std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request >)>	call_set_active_route_service_

std::shared_ptr< lanelet::projection::LocalFrameProjector >	map_projector_ = nullptr

std::string	georeference_ {""}

bool	starting_at_staging_area_

bool	enable_port_drayage_

std::string	previous_strategy_params_

const std::string	PORT_DRAYAGE_PLUGIN_ID = "port_drayage_plugin"

const std::string	PORT_DRAYAGE_STRATEGY_ID = "carma/port_drayage"

const std::string	SET_GUIDANCE_ACTIVE_SERVICE_ID = "/guidance/set_guidance_active"

rclcpp::Clock::SharedPtr	clock_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger_

Detailed Description

Implementation class for all the business logic of the PortDrayagePlugin

Definition at line 125 of file port_drayage_worker.hpp.

Constructor & Destructor Documentation

◆ PortDrayageWorker()

port_drayage_plugin::PortDrayageWorker::PortDrayageWorker	(	rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr	logger,
		rclcpp::Clock::SharedPtr	clock,
		std::function< void(carma_v2x_msgs::msg::MobilityOperation)>	mobility_operations_publisher,
		std::function< void(carma_msgs::msg::UIInstructions)>	ui_instructions_publisher,
		std::function< bool(std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request >)>	call_set_active_route
	)

Standard constructor for the PortDrayageWorker.

Parameters

mobility_operations_publisher	A function containing the logic necessary to publish a Mobility Operations message.
ui_instructions_publisher	A function containing the logic necessary to publish a UI Instructions message.
call_set_active_route	A function containing the logic necessary to call the SetActiveRoute service client.

Definition at line 43 of file port_drayage_worker.cpp.

        : logger_(logger), clock_(clock),
          publish_mobility_operation_(mobility_operations_publisher),
          publish_ui_instructions_(ui_instructions_publisher),
          call_set_active_route_service_(call_set_active_route),
          pdsm_(logger)
    {
        pdsm_.setOnArrivedAtDestinationCallback(std::bind(&PortDrayageWorker::onArrivedAtDestination, this));
        pdsm_.setOnReceivedNewDestinationCallback(std::bind(&PortDrayageWorker::onReceivedNewDestination, this));
    }

References onArrivedAtDestination(), onReceivedNewDestination(), pdsm_, port_drayage_plugin::PortDrayageStateMachine::setOnArrivedAtDestinationCallback(), and port_drayage_plugin::PortDrayageStateMachine::setOnReceivedNewDestinationCallback().

Here is the call graph for this function:

Member Function Documentation

◆ composeArrivalMessage()

carma_v2x_msgs::msg::MobilityOperation port_drayage_plugin::PortDrayageWorker::composeArrivalMessage ( ) const

Assemble the current dataset into a MobilityOperations message with a JSON formatted body containing CMV ID and cargo ID.

Definition at line 149 of file port_drayage_worker.cpp.

                                                                                      {
        carma_v2x_msgs::msg::MobilityOperation msg;
 
        msg.m_header.plan_id = "";
        msg.m_header.sender_id = cmv_id_;
        msg.m_header.recipient_id = "";
        msg.m_header.timestamp = clock_->now().nanoseconds()/1000000;
 
        msg.strategy = PORT_DRAYAGE_STRATEGY_ID;
 
        // Encode JSON with Boost Property Tree
        using boost::property_tree::ptree;
        ptree pt;
        pt.put("cmv_id", cmv_id_);
 
        // Add current vehicle location (latitude and longitude)
        ptree location;
        location.put("latitude", current_gps_position_.latitude);
        location.put("longitude", current_gps_position_.longitude);
        pt.put_child("location", location);
 
        // Add flag to indicate whether CMV is carring cargo
        pt.put("cargo", cargo_id_ != "");
 
        // If CMV has arrived at its initial destination, assign 'operation' field for its initial arrival message
        if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION) {
            // The CMV's initial destination is either the Staging Area Entrance or the Port Entrance
            if (starting_at_staging_area_) {
                OperationID pickup_operation = OperationID::ENTER_STAGING_AREA;
                pt.put("operation", pickup_operation);
            }
            else {
                OperationID enter_port_operation = OperationID::ENTER_PORT;
                pt.put("operation", enter_port_operation);
            }
 
            // Add cargo_id if CMV is carrying cargo
            if (cargo_id_ != "") {
                pt.put("cargo_id", cargo_id_);
            }
        }
        // If CMV has arrived at a received destination, add necessary fields based on the destination type that it has arrived at
        else if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) {
            // Assign the 'operation' using the 'operation' from the last received port drayage message
            pt.put("operation", latest_mobility_operation_msg_.operation);
 
            // Assign the 'action_id' using the 'action_id' from the last received port drayage message
            if (latest_mobility_operation_msg_.current_action_id) {
                pt.put("action_id", *latest_mobility_operation_msg_.current_action_id);
            }
            else {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a received destination, but does not have an action_id to broadcast.");
            }
 
            // Assign specific fields for arrival at a Pickup location
            if (latest_mobility_operation_msg_.operation == OperationID::PICKUP) {
                // Assign 'cargo_id' using the value received in the previous port drayage message
                if (latest_mobility_operation_msg_.cargo_id) {
                    pt.put("cargo_id", *latest_mobility_operation_msg_.cargo_id);
                }
                else {
                    RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at loading area, but does not have a cargo_id to broadcast.");
                }
 
                if (cargo_id_ != "") {
                    RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a loading area, but it is already carrying cargo.");
                }
            }
            // Assign specific fields for arrival at a Dropoff location
            else if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF) {
                // Assign 'cargo_id' using the ID of the cargo currently being carried
                if (cargo_id_ != "") {
                    pt.put("cargo_id", cargo_id_);
                }
                else {
                    RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV has arrived at a dropoff area, but does not have a cargo_id to broadcast.");
                }
            }
            // Assign 'cargo_id' using the ID of the cargo currently being carried if the CMV is not arriving at a Pickup location
            else {
                if (cargo_id_ != "") {
                    pt.put("cargo_id", cargo_id_);
                }
            }
        }
 
        std::stringstream body_stream;
        boost::property_tree::json_parser::write_json(body_stream, pt);
        msg.strategy_params = body_stream.str();
 
        return msg;
    }

Referenced by onArrivedAtDestination().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ composeSetActiveRouteRequest()

std::shared_ptr< carma_planning_msgs::srv::SetActiveRoute::Request > port_drayage_plugin::PortDrayageWorker::composeSetActiveRouteRequest	(	boost::optional< double >	dest_latitude,
		boost::optional< double >	dest_longitude
	)		const

Create a SetActiveRoute service request to set a new active route for the system based on the destination points contained in the most recently-received Port Drayage MobilityOperation message intended for this vehicle.

Parameters

dest_latitude	The destination point's latitude
dest_longitude	The destination point's longitude

Definition at line 98 of file port_drayage_worker.cpp.

                                                                                                                                                                                              {
        auto route_req = std::make_shared<carma_planning_msgs::srv::SetActiveRoute::Request>();
        if (dest_latitude && dest_longitude) {
            route_req->choice = carma_planning_msgs::srv::SetActiveRoute::Request::DESTINATION_POINTS_ARRAY;
            route_req->route_id = latest_mobility_operation_msg_.operation;
 
            carma_v2x_msgs::msg::Position3D destination_point;
            destination_point.latitude = *latest_mobility_operation_msg_.dest_latitude;
            destination_point.longitude = *latest_mobility_operation_msg_.dest_longitude;
            destination_point.elevation_exists = false;
 
            route_req->destination_points.push_back(destination_point);
        }
        else {
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "No destination points were received. Routing could not be completed.");
            throw std::invalid_argument("No destination points were received. Routing could not be completed");
        }
 
        return route_req;
    }

References port_drayage_plugin::PortDrayageMobilityOperationMsg::dest_latitude, port_drayage_plugin::PortDrayageMobilityOperationMsg::dest_longitude, latest_mobility_operation_msg_, logger_, and port_drayage_plugin::PortDrayageMobilityOperationMsg::operation.

Referenced by onReceivedNewDestination().

Here is the caller graph for this function:

◆ composeUIInstructions()

carma_msgs::msg::UIInstructions port_drayage_plugin::PortDrayageWorker::composeUIInstructions	(	const std::string &	current_operation,
		const std::string &	previous_operation
	)

Creates a UIInstructions message that can be used to create a pop-up on the Web UI to notify a user that a new route has been received for a specified destination type, and that the system can be engaged on that route.

Parameters

current_operation	The 'operation' identifier associated with the latest received and processed Port Drayage MobilityOperation message.
previous_operation	The previously completed 'operation' identifier. This is an empty string if no 'operation' was previously completed.

Definition at line 119 of file port_drayage_worker.cpp.

                                                                                                                                                {
        // Create the text that will be displayed in the Web UI popup
        std::string popup_text = "";
 
        // Add text that indicates the previous action was completed (if it was a pickup or dropoff action)
        if (previous_operation == OperationID::PICKUP) {
            popup_text += "The pickup action was completed successfully. ";
        }
        else if (previous_operation == OperationID::DROPOFF) {
            popup_text += "The dropoff action was completed successfully. ";
        }
        else if (previous_operation == OperationID::HOLDING_AREA) {
            popup_text += "The inspection was completed successfully. ";
        }
 
        // Add text to notify the user that the system can be engaged on the newly received route
        popup_text += "A new Port Drayage route with operation type '" + current_operation + "' has been received. "
                      "Select YES to engage the system on the route, or select NO to remain "
                      "disengaged.";
 
        // Create and populate the UI Instructions message
        carma_msgs::msg::UIInstructions ui_instructions_msg;
        ui_instructions_msg.stamp = clock_->now();
        ui_instructions_msg.msg = popup_text;
        ui_instructions_msg.type = carma_msgs::msg::UIInstructions::ACK_REQUIRED; // The popup will be displayed until the user interacts with it
        ui_instructions_msg.response_service = SET_GUIDANCE_ACTIVE_SERVICE_ID;
 
        return ui_instructions_msg;
    }

References clock_, port_drayage_plugin::OperationID::DROPOFF, port_drayage_plugin::OperationID::HOLDING_AREA, port_drayage_plugin::OperationID::PICKUP, and SET_GUIDANCE_ACTIVE_SERVICE_ID.

Referenced by onReceivedNewDestination().

Here is the caller graph for this function:

◆ getPortDrayageState()

PortDrayageState port_drayage_plugin::PortDrayageWorker::getPortDrayageState ( )

Get the current state of the port drayage state machine.

Returns: The current state value of the port drayage state machine

Definition at line 393 of file port_drayage_worker.cpp.

                                                            {
        return pdsm_.getState();
    }

References port_drayage_plugin::PortDrayageStateMachine::getState(), and pdsm_.

Here is the call graph for this function:

◆ mobilityOperationMessageParser()

void port_drayage_plugin::PortDrayageWorker::mobilityOperationMessageParser ( std::string mobility_operation_strategy_params )

Function to help parse the text included in an inbound MobilityOperation message's strategy_params field according to the JSON schema intended for MobilityOperation messages with strategy type 'carma/port_drayage'. Stores the parsed information in _latest_mobility_operation_msg.

Parameters

mobility_operation_strategy_params the strategy_params field of a MobilityOperation message

Definition at line 292 of file port_drayage_worker.cpp.

                                                                                                       {
        // Use Boost Property Tree to parse JSON-encoded strategy_params field in MobilityOperations message
        using boost::property_tree::ptree;
        ptree pt;
        std::istringstream mobility_operation_strategy_params_ss(mobility_operation_strategy_params);
        boost::property_tree::json_parser::read_json(mobility_operation_strategy_params_ss, pt);
 
        // Parse 'operation' field and assign the PortDrayageEvent type for this message accordingly
        latest_mobility_operation_msg_.operation = pt.get<std::string>("operation");
        RCLCPP_DEBUG_STREAM(logger_->get_logger(), "operation: " << latest_mobility_operation_msg_.operation);
 
        // If this CMV is commanded to pickup new cargo, check that it isn't already carrying cargo
        if (latest_mobility_operation_msg_.operation == OperationID::PICKUP && cargo_id_ != "") {
            RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'PICKUP' operation, but CMV is already carrying cargo.");
        }
 
        // If this CMV is commanded to dropoff cargo, check that it is actually carrying cargo
        if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF && cargo_id_ == "") {
            RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'DROPOFF' operation, but CMV isn't currently carrying cargo.");
        }
 
        // Parse 'cargo_id' field if it exists in strategy_params
        if (pt.count("cargo_id") != 0){
            latest_mobility_operation_msg_.cargo_id = pt.get<std::string>("cargo_id");
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "cargo id: " << *latest_mobility_operation_msg_.cargo_id);
 
            // Log message if the cargo ID being dropped off does not match the cargo ID currently being carried
            if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF && cargo_id_ != pt.get<std::string>("cargo_id")) {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "CMV commanded to dropoff an invalid Cargo ID. Currently carrying " << cargo_id_ << ", commanded to dropoff " << pt.get<std::string>("cargo_id"));
            }
        }
        else{
            // If this message is for 'PICKUP', then the 'cargo_id' field is required
            if(latest_mobility_operation_msg_.operation == OperationID::PICKUP) {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'PICKUP' operation, but no cargo_id was included.");
            }
            else if (latest_mobility_operation_msg_.operation == OperationID::DROPOFF) {
                RCLCPP_WARN_STREAM(logger_->get_logger(), "Received 'DROPOFF' operation, but no cargo_id was included.");
            }
 
            latest_mobility_operation_msg_.cargo_id = boost::optional<std::string>();
        }
 
        // Parse 'action_id' field  if it exists in strategy_params
        if (pt.count("action_id") != 0){
            latest_mobility_operation_msg_.current_action_id = pt.get<std::string>("action_id");
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "action id: " << *latest_mobility_operation_msg_.current_action_id);
        }
        else{
            latest_mobility_operation_msg_.current_action_id = boost::optional<std::string>();
        }
 
        // Parse starting longitude/latitude fields if 'location' field exists in strategy_params:
        if (pt.count("location") != 0){
            latest_mobility_operation_msg_.start_longitude = pt.get<double>("location.longitude");
            latest_mobility_operation_msg_.start_latitude = pt.get<double>("location.latitude");
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "start long: " << *latest_mobility_operation_msg_.start_longitude);
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "start lat: " << *latest_mobility_operation_msg_.start_latitude);
        }
        else {
            latest_mobility_operation_msg_.start_longitude = boost::optional<double>();
            latest_mobility_operation_msg_.start_latitude = boost::optional<double>();
        }
 
        // Parse destination longitude/latitude fields if 'destination' field exists in strategy_params:
        if (pt.count("destination") != 0) {
            latest_mobility_operation_msg_.dest_longitude = pt.get<double>("destination.longitude");
            latest_mobility_operation_msg_.dest_latitude = pt.get<double>("destination.latitude");
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "dest long: " << *latest_mobility_operation_msg_.dest_longitude);
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "dest lat: " << *latest_mobility_operation_msg_.dest_latitude);
        }
        else {
            latest_mobility_operation_msg_.dest_longitude = boost::optional<double>();
            latest_mobility_operation_msg_.dest_latitude = boost::optional<double>();
        }
    }

Referenced by onInboundMobilityOperation().

Here is the caller graph for this function:

◆ onArrivedAtDestination()

void port_drayage_plugin::PortDrayageWorker::onArrivedAtDestination ( )

Callback for usage by the PortDrayageStateMachine when the vehicle has arrived at a destination.

Definition at line 74 of file port_drayage_worker.cpp.

                                                   {
        carma_v2x_msgs::msg::MobilityOperation msg = composeArrivalMessage();
        publish_mobility_operation_(msg);
    }

References composeArrivalMessage(), and publish_mobility_operation_.

Referenced by PortDrayageWorker().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onGuidanceState()

void port_drayage_plugin::PortDrayageWorker::onGuidanceState ( const carma_planning_msgs::msg::GuidanceState::UniquePtr msg )

Callback to process the current status of the guidance state machine.

Parameters

msg	a received GuidanceState message

Definition at line 369 of file port_drayage_worker.cpp.

                                                                                            {
        // Drayage operations have started when the CMV has been engaged for the first time
        if ((msg->state == carma_planning_msgs::msg::GuidanceState::ENGAGED) && (pdsm_.getState() == PortDrayageState::INACTIVE) && enable_port_drayage_) {
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV has been engaged for the first time. Processing DRAYAGE_START event.");
            pdsm_.processEvent(PortDrayageEvent::DRAYAGE_START);
        }
    }

References port_drayage_plugin::DRAYAGE_START, enable_port_drayage_, lightbar_manager::ENGAGED, port_drayage_plugin::PortDrayageStateMachine::getState(), port_drayage_plugin::INACTIVE, logger_, pdsm_, and port_drayage_plugin::PortDrayageStateMachine::processEvent().

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onInboundMobilityOperation()

void port_drayage_plugin::PortDrayageWorker::onInboundMobilityOperation ( carma_v2x_msgs::msg::MobilityOperation::UniquePtr msg )

Callback to process a received MobilityOperation message.

Parameters

msg	a received MobilityOperation message

Definition at line 242 of file port_drayage_worker.cpp.

                                                                                                      {
        // Check if the received message is a new message for port drayage
        if((msg->strategy == PORT_DRAYAGE_STRATEGY_ID) && (msg->strategy_params != previous_strategy_params_)) {
            // Use Boost Property Tree to parse JSON-encoded strategy_params field in MobilityOperations message
            using boost::property_tree::ptree;
            ptree pt;
            std::istringstream strategy_params_ss(msg->strategy_params);
            boost::property_tree::json_parser::read_json(strategy_params_ss, pt);
 
            std::string mobility_operation_cmv_id = pt.get<std::string>("cmv_id");
 
            // Check if the received MobilityOperation message is intended for this vehicle's cmv_id
            if(mobility_operation_cmv_id == cmv_id_) {
                // Since a new message indicates the previous action was completed, update all cargo-related data members based on the previous action that was completed
                updateCargoInformationAfterActionCompletion(latest_mobility_operation_msg_);
 
                // Store the previously received operation
                previously_completed_operation_ = latest_mobility_operation_msg_.operation;
 
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Processing new port drayage MobilityOperation message for cmv_id " << mobility_operation_cmv_id);
                mobilityOperationMessageParser(msg->strategy_params);
                previous_strategy_params_ = msg->strategy_params;
 
                pdsm_.processEvent(PortDrayageEvent::RECEIVED_NEW_DESTINATION);
            }
            else {
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Ignoring received port drayage MobilityOperation message intended for cmv_id " << mobility_operation_cmv_id);
            }
        }
    }

References cmv_id_, latest_mobility_operation_msg_, logger_, mobilityOperationMessageParser(), port_drayage_plugin::PortDrayageMobilityOperationMsg::operation, pdsm_, PORT_DRAYAGE_STRATEGY_ID, previous_strategy_params_, previously_completed_operation_, port_drayage_plugin::PortDrayageStateMachine::processEvent(), port_drayage_plugin::RECEIVED_NEW_DESTINATION, and updateCargoInformationAfterActionCompletion().

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onNewGeoreference()

void port_drayage_plugin::PortDrayageWorker::onNewGeoreference ( std_msgs::msg::String::UniquePtr msg )

Callback for map projection string to define lat/lon <--> map conversion.

Parameters

msg	The proj string defining the projection.

Definition at line 411 of file port_drayage_worker.cpp.

                                                                            {
        // Build projector from proj string
        if (georeference_ != msg->data)
        {
            georeference_ = msg->data;
            map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str());
        }
    }

References georeference_, and map_projector_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure().

Here is the caller graph for this function:

◆ onNewPose()

void port_drayage_plugin::PortDrayageWorker::onNewPose ( geometry_msgs::msg::PoseStamped::UniquePtr msg )

Callback for the pose subscriber. The pose will be converted into lat/lon and stored locally.

Parameters

msg	Latest pose message

Definition at line 397 of file port_drayage_worker.cpp.

                                                                              {
        if (!map_projector_) {
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Ignoring pose message as projection string has not been defined");
            return;
        }
 
        // Convert pose message contents to a GPS coordinate
        lanelet::GPSPoint coord = map_projector_->reverse( { msg->pose.position.x, msg->pose.position.y, msg->pose.position.z } );
 
        // Update the locally-stored GPS position of the CMV
        current_gps_position_.latitude = coord.lat;
        current_gps_position_.longitude = coord.lon;
    }

References current_gps_position_, port_drayage_plugin::LatLonCoordinate::latitude, logger_, port_drayage_plugin::LatLonCoordinate::longitude, and map_projector_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure().

Here is the caller graph for this function:

◆ onReceivedNewDestination()

void port_drayage_plugin::PortDrayageWorker::onReceivedNewDestination ( )

Callback for usage by the PortDrayageStateMachine when the vehicle has received a new destination.

Definition at line 79 of file port_drayage_worker.cpp.

                                                     {
        //  Populate the service request with the destination coordinates from the last received port drayage mobility operation message
        auto route_req = composeSetActiveRouteRequest(latest_mobility_operation_msg_.dest_latitude, latest_mobility_operation_msg_.dest_longitude);
 
        // Call service client to set the new active route
        bool is_route_generation_successful = call_set_active_route_service_(route_req);
 
        if (is_route_generation_successful) {
            // Publish UI Instructions to trigger a pop-up on the Web UI for the user to engage on the newly received route if desired
            carma_msgs::msg::UIInstructions ui_instructions_msg = composeUIInstructions(latest_mobility_operation_msg_.operation, previously_completed_operation_);
            publish_ui_instructions_(ui_instructions_msg);
        }
        else {
            // Throw exception if route generation was not successful
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Route generation failed. Routing could not be completed.");
            throw std::invalid_argument("Route generation failed. Routing could not be completed.");
        }
    }

References call_set_active_route_service_, composeSetActiveRouteRequest(), composeUIInstructions(), port_drayage_plugin::PortDrayageMobilityOperationMsg::dest_latitude, port_drayage_plugin::PortDrayageMobilityOperationMsg::dest_longitude, latest_mobility_operation_msg_, logger_, port_drayage_plugin::PortDrayageMobilityOperationMsg::operation, previously_completed_operation_, and publish_ui_instructions_.

Referenced by PortDrayageWorker().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ onRouteEvent()

void port_drayage_plugin::PortDrayageWorker::onRouteEvent ( const carma_planning_msgs::msg::RouteEvent::UniquePtr msg )

Callback to process each Route Event.

Parameters

msg	a received RouteEvent message

Definition at line 377 of file port_drayage_worker.cpp.

                                                                                      {
        // CMV has officially arrived at its destination if the previous route was completed and is no longer active
        if (latest_route_event_ != nullptr) {
            if (latest_route_event_->event == carma_planning_msgs::msg::RouteEvent::ROUTE_COMPLETED && msg->event == carma_planning_msgs::msg::RouteEvent::ROUTE_LOADED) {
                if (pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_INITIAL_DESTINATION || pdsm_.getState() == PortDrayageState::EN_ROUTE_TO_RECEIVED_DESTINATION) {
                    RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed its previous route, and the previous route is no longer active.");
                    RCLCPP_DEBUG_STREAM(logger_->get_logger(), "Processing ARRIVED_AT_DESTINATION event.");
                    pdsm_.processEvent(PortDrayageEvent::ARRIVED_AT_DESTINATION);
                }
            }
        }
 
        // Update the latest received route event data member
        latest_route_event_ = std::move(msg);
    }

References port_drayage_plugin::ARRIVED_AT_DESTINATION, port_drayage_plugin::EN_ROUTE_TO_INITIAL_DESTINATION, port_drayage_plugin::EN_ROUTE_TO_RECEIVED_DESTINATION, port_drayage_plugin::PortDrayageStateMachine::getState(), latest_route_event_, logger_, pdsm_, and port_drayage_plugin::PortDrayageStateMachine::processEvent().

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ setCargoID()

void port_drayage_plugin::PortDrayageWorker::setCargoID ( const std::string & cargo_id )

Setter function to set this object's cargo_id_ string.

Parameters

cargo_id The provided Cargo ID for this object

Definition at line 62 of file port_drayage_worker.cpp.

                                                                {
        cargo_id_ = cargo_id;
    }

References cargo_id_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure(), and port_drayage_plugin::PortDrayagePlugin::parameter_update_callback().

Here is the caller graph for this function:

◆ setEnablePortDrayageFlag()

void port_drayage_plugin::PortDrayageWorker::setEnablePortDrayageFlag ( bool enable_port_drayage )

Setter function to set this object's enable_port_drayage flag.

Parameters

enable_port_drayage The provided boolean flag

Definition at line 66 of file port_drayage_worker.cpp.

                                                                             {
        enable_port_drayage_ = enable_port_drayage;
    }

References enable_port_drayage_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure(), and port_drayage_plugin::PortDrayagePlugin::parameter_update_callback().

Here is the caller graph for this function:

◆ setStartingAtStagingAreaFlag()

void port_drayage_plugin::PortDrayageWorker::setStartingAtStagingAreaFlag ( bool starting_at_staging_area )

Setter function to set this object's starting_at_staging_area flag.

Parameters

starting_at_staging_area The provided boolean flag

Definition at line 70 of file port_drayage_worker.cpp.

                                                                                      {
        starting_at_staging_area_ = starting_at_staging_area;
    }

References starting_at_staging_area_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure(), and port_drayage_plugin::PortDrayagePlugin::parameter_update_callback().

Here is the caller graph for this function:

◆ setVehicleID()

void port_drayage_plugin::PortDrayageWorker::setVehicleID ( const std::string & cmv_id )

Setter function to set this object's cmv_id_ string.

Parameters

cmv_id The provided CMV ID for this object

Definition at line 58 of file port_drayage_worker.cpp.

                                                                {
        cmv_id_ = cmv_id;
    }

References cmv_id_.

Referenced by port_drayage_plugin::PortDrayagePlugin::handle_on_configure(), and port_drayage_plugin::PortDrayagePlugin::parameter_update_callback().

Here is the caller graph for this function:

◆ updateCargoInformationAfterActionCompletion()

void port_drayage_plugin::PortDrayageWorker::updateCargoInformationAfterActionCompletion ( const PortDrayageMobilityOperationMsg & previous_port_drayage_msg )

Method to update worker's cargo-related data members depending on whether the previously completed action was for a pickup or dropoff.

Parameters

previous_port_drayage_msg The contents of the previously received MobilityOperation port drayage message for this CMV stored in a PortDrayageMobilityOperationMsg object.

Definition at line 273 of file port_drayage_worker.cpp.

                                                                                                                                        {
        // If the previously received message was for 'Pickup' or 'Dropoff', update this object's cargo_id_ member accordingly
        // Note: This assumes the previous 'Pickup' or 'Dropoff' action was successful
        if (previous_port_drayage_msg.operation == OperationID::PICKUP) {
 
            if (previous_port_drayage_msg.cargo_id) {
                cargo_id_ = *previous_port_drayage_msg.cargo_id;
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed pickup action. CMV is now carrying cargo ID " << cargo_id_);
            }
            else {
                RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV has completed pickup, but there is no Cargo ID associated with the picked up cargo.");
            }
        }
        else if (previous_port_drayage_msg.operation == OperationID::DROPOFF) {
            RCLCPP_DEBUG_STREAM(logger_->get_logger(), "CMV completed dropoff action. CMV is no longer carrying cargo ID " << cargo_id_);
            cargo_id_ = ""; // Empty string is used when no cargo is being carried
        }
    }

References port_drayage_plugin::PortDrayageMobilityOperationMsg::cargo_id, cargo_id_, port_drayage_plugin::OperationID::DROPOFF, logger_, port_drayage_plugin::PortDrayageMobilityOperationMsg::operation, and port_drayage_plugin::OperationID::PICKUP.

Referenced by onInboundMobilityOperation().

Here is the caller graph for this function:

Member Data Documentation

◆ call_set_active_route_service_

std::function<bool(std::shared_ptr<carma_planning_msgs::srv::SetActiveRoute::Request>)> port_drayage_plugin::PortDrayageWorker::call_set_active_route_service_

private

Definition at line 135 of file port_drayage_worker.hpp.

Referenced by onReceivedNewDestination().

◆ cargo_id_

std::string port_drayage_plugin::PortDrayageWorker::cargo_id_

private

Definition at line 132 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), mobilityOperationMessageParser(), setCargoID(), and updateCargoInformationAfterActionCompletion().

◆ clock_

rclcpp::Clock::SharedPtr port_drayage_plugin::PortDrayageWorker::clock_

private

Definition at line 150 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), and composeUIInstructions().

◆ cmv_id_

std::string port_drayage_plugin::PortDrayageWorker::cmv_id_

private

Definition at line 131 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), onInboundMobilityOperation(), and setVehicleID().

◆ current_gps_position_

LatLonCoordinate port_drayage_plugin::PortDrayageWorker::current_gps_position_

Definition at line 288 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), and onNewPose().

◆ enable_port_drayage_

bool port_drayage_plugin::PortDrayageWorker::enable_port_drayage_

private

Definition at line 139 of file port_drayage_worker.hpp.

Referenced by onGuidanceState(), and setEnablePortDrayageFlag().

◆ georeference_

std::string port_drayage_plugin::PortDrayageWorker::georeference_ {""}

private

Definition at line 137 of file port_drayage_worker.hpp.

Referenced by onNewGeoreference().

◆ latest_mobility_operation_msg_

PortDrayageMobilityOperationMsg port_drayage_plugin::PortDrayageWorker::latest_mobility_operation_msg_

Definition at line 285 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), composeSetActiveRouteRequest(), mobilityOperationMessageParser(), onInboundMobilityOperation(), and onReceivedNewDestination().

◆ latest_route_event_

std::shared_ptr<carma_planning_msgs::msg::RouteEvent> port_drayage_plugin::PortDrayageWorker::latest_route_event_ = nullptr

private

Definition at line 128 of file port_drayage_worker.hpp.

Referenced by onRouteEvent().

◆ logger_

rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr port_drayage_plugin::PortDrayageWorker::logger_

private

Definition at line 153 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), composeSetActiveRouteRequest(), mobilityOperationMessageParser(), onGuidanceState(), onInboundMobilityOperation(), onNewPose(), onReceivedNewDestination(), onRouteEvent(), and updateCargoInformationAfterActionCompletion().

◆ map_projector_

std::shared_ptr<lanelet::projection::LocalFrameProjector> port_drayage_plugin::PortDrayageWorker::map_projector_ = nullptr

private

Definition at line 136 of file port_drayage_worker.hpp.

Referenced by onNewGeoreference(), and onNewPose().

◆ pdsm_

PortDrayageStateMachine port_drayage_plugin::PortDrayageWorker::pdsm_

private

Definition at line 129 of file port_drayage_worker.hpp.

Referenced by PortDrayageWorker(), composeArrivalMessage(), getPortDrayageState(), onGuidanceState(), onInboundMobilityOperation(), and onRouteEvent().

◆ PORT_DRAYAGE_PLUGIN_ID

const std::string port_drayage_plugin::PortDrayageWorker::PORT_DRAYAGE_PLUGIN_ID = "port_drayage_plugin"

private

Definition at line 145 of file port_drayage_worker.hpp.

◆ PORT_DRAYAGE_STRATEGY_ID

const std::string port_drayage_plugin::PortDrayageWorker::PORT_DRAYAGE_STRATEGY_ID = "carma/port_drayage"

private

Definition at line 146 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), and onInboundMobilityOperation().

◆ previous_strategy_params_

std::string port_drayage_plugin::PortDrayageWorker::previous_strategy_params_

private

Definition at line 142 of file port_drayage_worker.hpp.

Referenced by onInboundMobilityOperation().

◆ previously_completed_operation_

std::string port_drayage_plugin::PortDrayageWorker::previously_completed_operation_

private

Definition at line 130 of file port_drayage_worker.hpp.

Referenced by onInboundMobilityOperation(), and onReceivedNewDestination().

◆ publish_mobility_operation_

std::function<void(carma_v2x_msgs::msg::MobilityOperation)> port_drayage_plugin::PortDrayageWorker::publish_mobility_operation_

private

Definition at line 133 of file port_drayage_worker.hpp.

Referenced by onArrivedAtDestination().

◆ publish_ui_instructions_

std::function<void(carma_msgs::msg::UIInstructions)> port_drayage_plugin::PortDrayageWorker::publish_ui_instructions_

private

Definition at line 134 of file port_drayage_worker.hpp.

Referenced by onReceivedNewDestination().

◆ SET_GUIDANCE_ACTIVE_SERVICE_ID

const std::string port_drayage_plugin::PortDrayageWorker::SET_GUIDANCE_ACTIVE_SERVICE_ID = "/guidance/set_guidance_active"

private

Definition at line 147 of file port_drayage_worker.hpp.

Referenced by composeUIInstructions().

◆ starting_at_staging_area_

bool port_drayage_plugin::PortDrayageWorker::starting_at_staging_area_

private

Definition at line 138 of file port_drayage_worker.hpp.

Referenced by composeArrivalMessage(), and setStartingAtStagingAreaFlag().

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

port_drayage_plugin/include/port_drayage_plugin/port_drayage_worker.hpp
port_drayage_plugin/src/port_drayage_worker.cpp

Public Member Functions

Public Attributes

Private Attributes

Detailed Description

Constructor & Destructor Documentation

◆ PortDrayageWorker()

Member Function Documentation

◆ composeArrivalMessage()

◆ composeSetActiveRouteRequest()

◆ composeUIInstructions()

◆ getPortDrayageState()

◆ mobilityOperationMessageParser()

◆ onArrivedAtDestination()

◆ onGuidanceState()

◆ onInboundMobilityOperation()

◆ onNewGeoreference()

◆ onNewPose()

◆ onReceivedNewDestination()

◆ onRouteEvent()

◆ setCargoID()

◆ setEnablePortDrayageFlag()

◆ setStartingAtStagingAreaFlag()

◆ setVehicleID()

◆ updateCargoInformationAfterActionCompletion()

Member Data Documentation

◆ call_set_active_route_service_

◆ cargo_id_

◆ clock_

◆ cmv_id_

◆ current_gps_position_

◆ enable_port_drayage_

◆ georeference_

◆ latest_mobility_operation_msg_

◆ latest_route_event_

◆ logger_

◆ map_projector_

◆ pdsm_

◆ PORT_DRAYAGE_PLUGIN_ID

◆ PORT_DRAYAGE_STRATEGY_ID

◆ previous_strategy_params_

◆ previously_completed_operation_

◆ publish_mobility_operation_

◆ publish_ui_instructions_

◆ SET_GUIDANCE_ACTIVE_SERVICE_ID

◆ starting_at_staging_area_