[ROS2] 使用LifecycleNode管理节点起停等状态

  
/// Callback function for configure transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_configure(const State & previous_state);

  

/// Callback function for cleanup transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_cleanup(const State & previous_state);

  

/// Callback function for shutdown transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_shutdown(const State & previous_state);

  

/// Callback function for activate transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_activate(const State & previous_state);

  

/// Callback function for deactivate transition

/*

* \return true by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_deactivate(const State & previous_state);

  

/// Callback function for errorneous transition

/*

* \return false by default

*/

RCLCPP_LIFECYCLE_PUBLIC

virtual CallbackReturn

on_error(const State & previous_state);

enum class CallbackReturn : uint8_t

{

SUCCESS = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_SUCCESS,

FAILURE = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_FAILURE,

ERROR = lifecycle_msgs::msg::Transition::TRANSITION_CALLBACK_ERROR

};

#include <chrono>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
#include <utility>

#include "lifecycle_msgs/msg/transition.hpp"

#include "rclcpp/rclcpp.hpp"
#include "rclcpp/publisher.hpp"

#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "rclcpp_lifecycle/lifecycle_publisher.hpp"

#include "rcutils/logging_macros.h"

#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

class ManagedScan : public rclcpp_lifecycle::LifecycleNode
{
public:

  explicit ManagedScan(const std::string & node_name, bool intra_process_comms = false)
  : rclcpp_lifecycle::LifecycleNode(node_name,
      rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms))
  {}

  void
  publish()
  {
    static size_t count = 0;
    auto msg = std::make_unique<std_msgs::msg::String>();
    msg->data = "Lifecycle HelloWorld #" + std::to_string(++count);

    if (!pub_->is_activated()) {
      RCLCPP_INFO(
        get_logger(), "Lifecycle publisher is currently inactive. Messages are not published.");
    } else {
      RCLCPP_INFO(
        get_logger(), "Lifecycle publisher is active. Publishing: [%s]", msg->data.c_str());
    }

    pub_->publish(std::move(msg));
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State &)
  {
    pub_ = this->create_publisher<std_msgs::msg::String>("managed_scan", 10);
    timer_ = this->create_wall_timer(
      1s, std::bind(&ManagedScan::publish, this));

    RCLCPP_INFO(get_logger(), "on_configure() is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }


  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State &)
  {
    pub_->on_activate();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called.");

    std::this_thread::sleep_for(2s);

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State &)
  {
    pub_->on_deactivate();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State &)
  {

    timer_.reset();
    pub_.reset();

    RCUTILS_LOG_INFO_NAMED(get_name(), "on cleanup is called.");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }

  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_shutdown(const rclcpp_lifecycle::State & state)
  {

    timer_.reset();    
    pub_.reset();

    RCUTILS_LOG_INFO_NAMED(
      get_name(),
      "on shutdown is called from state %s.",
      state.label().c_str());

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
  }
    
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_error(const rclcpp_lifecycle::State &)
  {

    RCUTILS_LOG_INFO_NAMED(get_name(), "something went wrong!");

    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
  }

private:
  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;

  std::shared_ptr<rclcpp::TimerBase> timer_;
};


int main(int argc, char * argv[])
{
  // force flush of the stdout buffer.
  // this ensures a correct sync of all prints
  // even when executed simultaneously within the launch file.
  setvbuf(stdout, NULL, _IONBF, BUFSIZ);

  rclcpp::init(argc, argv);

  rclcpp::executors::SingleThreadedExecutor exe;

  std::shared_ptr<ManagedScan> lc_node =
    std::make_shared<ManagedScan>("managed_scan_node");

  exe.add_node(lc_node->get_node_base_interface());

  exe.spin();

  rclcpp::shutdown();

  return 0;
}

git clone https://gitee.com/shoufei/lifecycle_node_demo.git

std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::String>> pub_;

ros2 launch lifecycle_node_demo lifecycle_demo.launch.py

/lifecycle_node_demo_node/transition_event

ros2 lifecycle get /lifecycle_node_demo_node

unconfigured [1]

ros2 lifecycle set /lifecycle_node_demo_node configure

Transitioning successful

timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 1
  label: unconfigured
goal_state:
  id: 10
  label: configuring
---
timestamp: 0
transition:
  id: 0
  label: ''
start_state:
  id: 10
  label: configuring
goal_state:
  id: 2
  label: inactive
---

# Copyright (c) 2018 Intel Corporation

#

# 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.

  

import os

  

from ament_index_python.packages import get_package_share_directory

  

from launch import LaunchDescription

from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable

from launch.substitutions import LaunchConfiguration

from launch_ros.actions import Node

from nav2_common.launch import RewrittenYaml

  
  

def generate_launch_description():

# Get the launch directory

bringup_dir = get_package_share_directory('nav2_bringup')

  

namespace = LaunchConfiguration('namespace')

use_sim_time = LaunchConfiguration('use_sim_time')

autostart = LaunchConfiguration('autostart')

params_file = LaunchConfiguration('params_file')

  

lifecycle_nodes = ['controller_server',

'planner_server',

'recoveries_server',

'bt_navigator',

'waypoint_follower']

  

# Map fully qualified names to relative ones so the node's namespace can be prepended.

# In case of the transforms (tf), currently, there doesn't seem to be a better alternative

# https://github.com/ros/geometry2/issues/32

# https://github.com/ros/robot_state_publisher/pull/30

# TODO(orduno) Substitute with `PushNodeRemapping`

# https://github.com/ros2/launch_ros/issues/56

remappings = [('/tf', 'tf'),

('/tf_static', 'tf_static')]

  

# Create our own temporary YAML files that include substitutions

param_substitutions = {

'use_sim_time': use_sim_time,

'autostart': autostart}

  

configured_params = RewrittenYaml(

source_file=params_file,

root_key=namespace,

param_rewrites=param_substitutions,

convert_types=True)

  

return LaunchDescription([

# Set env var to print messages to stdout immediately

SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),

  

DeclareLaunchArgument(

'namespace', default_value='',

description='Top-level namespace'),

  

DeclareLaunchArgument(

'use_sim_time', default_value='false',

description='Use simulation (Gazebo) clock if true'),

  

DeclareLaunchArgument(

'autostart', default_value='true',

description='Automatically startup the nav2 stack'),

  

DeclareLaunchArgument(

'params_file',

default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),

description='Full path to the ROS2 parameters file to use'),

  

Node(

package='nav2_controller',

executable='controller_server',

output='screen',

parameters=[configured_params],

remappings=remappings),

  

Node(

package='nav2_planner',

executable='planner_server',

name='planner_server',

output='screen',

parameters=[configured_params],

remappings=remappings),

  

Node(

package='nav2_recoveries',

executable='recoveries_server',

name='recoveries_server',

output='screen',

parameters=[configured_params],

remappings=remappings),

  

Node(

package='nav2_bt_navigator',

executable='bt_navigator',

name='bt_navigator',

output='screen',

parameters=[configured_params],

remappings=remappings),

  

Node(

package='nav2_waypoint_follower',

executable='waypoint_follower',

name='waypoint_follower',

output='screen',

parameters=[configured_params],

remappings=remappings),

  

Node(

package='nav2_lifecycle_manager',

executable='lifecycle_manager',

name='lifecycle_manager_navigation',

output='screen',

parameters=[{'use_sim_time': use_sim_time},

{'autostart': autostart},

{'node_names': lifecycle_nodes}]),

  

])

lifecycle_nodes = ['controller_server',

'planner_server',

'recoveries_server',

'bt_navigator',

'waypoint_follower']