Class OperationsManager

Class Documentation

class OperationsManager

Manages operations plugins to call on route tracking.

Public Types

typedef std::vector<RouteOperation::Ptr>::const_iterator OperationsIter

Public Functions

explicit OperationsManager(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_subscriber)

A constructor for nav2_route::OperationsManager.

~OperationsManager() = default

A Destructor for nav2_route::OperationsManager.

OperationPtrs findGraphOperations(const NodePtr node, const EdgePtr edge_enter, const EdgePtr edge_exit)

Finds the set of operations stored in the graph to trigger at this transition.

Parameters:
  • nodeNode to check

  • edge_entered – Edge entered to check for ON_ENTER events

  • edge_exit – Edge exit to check for ON_EXIT events

Returns:

OperationPtrs A vector of operation pointers to execute

template<typename T>
void findGraphOperationsToProcess(T &obj, const OperationTrigger &trigger, OperationPtrs &operations)

Finds the set of operations stored in graph objects, by event.

Parameters:
  • node – op_vec Operations vector to check

  • trigger – Trigger for which operations in op_vec should be included

  • operations – Output vector populated with relevant operations

void updateResult(const std::string &name, const OperationResult &op_result, OperationsResult &result)

Updates manager result state by an individual operation’s result.

Parameters:
  • name – Operations’ name

  • op_result – Operations’ result

  • result – Manager’s result to update

OperationsResult process(const bool status_change, const RouteTrackingState &state, const Route &route, const geometry_msgs::msg::PoseStamped &pose, const ReroutingState &rerouting_info)

Processes the operations at this tracker state.

Parameters:
  • status_change – Whether something meaningful has changed

  • state – The route tracking state to check for state info

  • route – The raw route being tracked

  • pose – robot pose

  • rerouting_info – Rerouting information regarding previous partial state

Returns:

A result vector whether the operations are requesting something to occur

Protected Functions

void processOperationsPluginVector(const std::vector<RouteOperation::Ptr> &operations, OperationsResult &result, const NodePtr node, const EdgePtr edge_entered, const EdgePtr edge_exited, const Route &route, const geometry_msgs::msg::PoseStamped &pose)

Processes a vector of operations plugins, by trigger.

Parameters:
  • operations – Operations to trigger

  • Results – to populate from operations

Protected Attributes

pluginlib::ClassLoader<RouteOperation> plugin_loader_
std::unordered_map<std::string, RouteOperation::Ptr> graph_operations_
std::vector<RouteOperation::Ptr> change_operations_
std::vector<RouteOperation::Ptr> query_operations_
rclcpp::Logger logger_ = {rclcpp::get_logger("OperationsManager")}