Nav2 源码(2): LifecycleManager 如何处理导航节点的启动与状态切换

static const char * pub_transition_event_topic = "~/transition_event";static const char * srv_change_state_service = "~/change_state";static const char * srv_get_state_service = "~/get_state";static const char * srv_get_available_states_service = "~/get_available_states";static const char * srv_get_available_transitions_service = "~/get_available_transitions";static const char * srv_get_transition_graph = "~/get_transition_graph";

~/src/ros2/rcl/rcl_lifecycle/src/com_interface.c

pub_transition_event_topic 

classLifecycleServiceClient{public:  explicit     LifecycleServiceClient(const std::string & lifecycle_node_name);  LifecycleServiceClient(    const std::string & lifecycle_node_name,    rclcpp::Node::SharedPtr parent_node);  /// Trigger a state change  /**   * 状态转换   * transition 状态，定义在 lifecycle_msgs::msg::Transition   */  bool change_state(    const uint8_t transition,    const std::chrono::seconds timeout);  bool change_state(std::uint8_t transition);  /**   *  获取当前状态，状态值定义在 lifecycle_msgs::msg::State   */  uint8_t get_state(const std::chrono::seconds timeout = std::chrono::seconds(2));protected:  rclcpp::Node::SharedPtr node_;  ServiceClient<lifecycle_msgs::srv::ChangeState> change_state_;  ServiceClient<lifecycle_msgs::srv::GetState> get_state_;};

template<class ServiceT>class ServiceClient{public:  explicitServiceClient(    const std::string & service_name,    const rclcpp::Node::SharedPtr & provided_node)  : service_name_(service_name), node_(provided_node)  {    callback_group_ = node_->create_callback_group(      rclcpp::CallbackGroupType::MutuallyExclusive,      false);    callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface());    client_ = node_->create_client<ServiceT>(      service_name,      rclcpp::ServicesQoS().get_rmw_qos_profile(),      callback_group_);  }  using RequestType = typename ServiceT::Request;  using ResponseType = typename ServiceT::Response;   /// 省略部分代码  /**  * @brief 执行 request 请求  */  boolinvoke(    typename RequestType::SharedPtr & request,    typename ResponseType::SharedPtr & response)  {    while (!client_->wait_for_service(std::chrono::seconds(1))) {      if (!rclcpp::ok()) {        throw std::runtime_error(                service_name_ + " service client: interrupted while waiting for service");      }    }    auto future_result = client_->async_send_request(request);    if (callback_group_executor_.spin_until_future_complete(future_result) !=      rclcpp::FutureReturnCode::SUCCESS)    {      // Pending request must be manually cleaned up if execution is interrupted or timed out      client_->remove_pending_request(future_result);      return false;    }    response = future_result.get();    return response.get();  }  /// 省略部分代码protected:  std::string service_name_;  rclcpp::Node::SharedPtr node_;  rclcpp::CallbackGroup::SharedPtr callback_group_;  rclcpp::executors::SingleThreadedExecutor callback_group_executor_;  typename rclcpp::Client<ServiceT>::SharedPtr client_;};

LifecycleServiceClient::LifecycleServiceClient(  const string & lifecycle_node_name,  rclcpp::Node::SharedPtr parent_node): node_(parent_node),  change_state_(lifecycle_node_name + "/change_state", node_),  get_state_(lifecycle_node_name + "/get_state", node_){  // Block until server is up  rclcpp::Rate r(20);  while(!get_state_.wait_for_service(2s)) {    RCLCPP_INFO(      node_->get_logger(), "Waiting for service %s...", get_state_.getServiceName().c_str());    r.sleep();  }}

bool LifecycleServiceClient::change_state(  std::uint8_t transition){  if (!change_state_.wait_for_service(5s)) {    throw std::runtime_error("change_state service is not available!");  }  auto request = std::make_shared<lifecycle_msgs::srv::ChangeState::Request>();  auto response = std::make_shared<lifecycle_msgs::srv::ChangeState::Response>();  request->transition.id = transition;  return change_state_.invoke(request, response);}

bool LifecycleManager::startup(){  message("Starting managed nodes bringup...");  if(    !changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||    !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))  {    RCLCPP_ERROR(get_logger(), "Failed to bring up all requested nodes. Aborting bringup.");    return false;  }  message("Managed nodes are active");  system_active_ = true;  createBondTimer();  return true;}

boolLifecycleManager::changeStateForAllNodes(std::uint8_t transition, bool hard_change){  // Hard change will continue even if a node fails  if(transition == Transition::TRANSITION_CONFIGURE ||    transition == Transition::TRANSITION_ACTIVATE)  {    for(auto & node_name : node_names_) {      try {        if(!changeStateForNode(node_name, transition)             && !hard_change) {          return false;        }      } catch(const std::runtime_error & e) {        RCLCPP_ERROR(          get_logger(),          "Failed to change state for node: %s. Exception: %s.", node_name.c_str(), e.what());        return false;      }    }  } else {    // 省略部分代码  }  return true;}

bool LifecycleManager::changeStateForNode(const std::string & node_name, std::uint8_t transition){  message(transition_label_map_[transition] + node_name);  if(!node_map_[node_name]->change_state(transition) ||    !(node_map_[node_name]->get_state() == transition_state_map_[transition]))  {    RCLCPP_ERROR(get_logger(), "Failed to change state for node: %s", node_name.c_str());    return false;  }  if(transition == Transition::TRANSITION_ACTIVATE) {    return createBondConnection(node_name);  } else if (transition == Transition::TRANSITION_DEACTIVATE) {    bond_map_.erase(node_name);  }  return true;}

std::map<std::string,   std::shared_ptr<nav2_util::LifecycleServiceClient>> node_map_;

node_map_[node_name]->change_state(transition)