From edaa8ec551bdfc6e2e1d82bdf61f37747451a05a Mon Sep 17 00:00:00 2001 From: YusukeKato Date: Wed, 11 Oct 2023 17:02:42 +0900 Subject: [PATCH] =?UTF-8?q?lifecycle=5Fnode=5Fmanager.cpp=E3=82=92?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- raspimouse_fake/CMakeLists.txt | 6 - .../src/lifecycle_node_manager.cpp | 208 ------------------ .../raspimouse_with_emptyworld.launch.py | 12 +- 3 files changed, 1 insertion(+), 225 deletions(-) delete mode 100644 raspimouse_fake/src/lifecycle_node_manager.cpp diff --git a/raspimouse_fake/CMakeLists.txt b/raspimouse_fake/CMakeLists.txt index 6738e61..a49a6fd 100644 --- a/raspimouse_fake/CMakeLists.txt +++ b/raspimouse_fake/CMakeLists.txt @@ -21,11 +21,6 @@ ament_target_dependencies(fake_raspimouse_component std_srvs) rclcpp_components_register_nodes(fake_raspimouse_component "fake_raspimouse::Raspimouse") -add_executable(lifecycle_node_manager src/lifecycle_node_manager.cpp) -ament_target_dependencies(lifecycle_node_manager - rclcpp - rclcpp_lifecycle) - ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies(rclcpp) @@ -50,7 +45,6 @@ install(TARGETS INCLUDES DESTINATION include) install(TARGETS - lifecycle_node_manager DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY diff --git a/raspimouse_fake/src/lifecycle_node_manager.cpp b/raspimouse_fake/src/lifecycle_node_manager.cpp deleted file mode 100644 index 706cba7..0000000 --- a/raspimouse_fake/src/lifecycle_node_manager.cpp +++ /dev/null @@ -1,208 +0,0 @@ -// The MIT License (MIT) -// -// Copyright 2023 RT Corporation -// -// Permission is hereby granted, free of charge, to any person obtaining a copy of -// this software and associated documentation files (the "Software"), to deal in -// the Software without restriction, including without limitation the rights to -// use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of -// the Software, and to permit persons to whom the Software is furnished to do so, -// subject to the following conditions: -// -// The above copyright notice and this permission notice shall be included in all -// copies or substantial portions of the Software. -// -// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS -// FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR -// COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER -// IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN -// CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. - - -#include -#include -#include -#include -#include - -#include "lifecycle_msgs/msg/state.hpp" -#include "lifecycle_msgs/msg/transition.hpp" -#include "lifecycle_msgs/srv/change_state.hpp" -#include "lifecycle_msgs/srv/get_state.hpp" - -#include "rclcpp/rclcpp.hpp" - - -using namespace std::chrono_literals; -using MsgState = lifecycle_msgs::msg::State; -using MsgTransition = lifecycle_msgs::msg::Transition; -using SrvGetState = lifecycle_msgs::srv::GetState; -using SrvChangeState = lifecycle_msgs::srv::ChangeState; - -std::uint8_t state_of( - std::string target_node_name, - rclcpp::Node::SharedPtr node, std::chrono::seconds time_out = 10s) -{ - auto request = std::make_shared(); - auto service_name = target_node_name + "/get_state"; - auto client = node->create_client(service_name); - - if (!client->wait_for_service(time_out)) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s is not avaliable.", service_name.c_str()); - return MsgState::PRIMARY_STATE_UNKNOWN; - } - - auto future_result = client->async_send_request(request); - auto future_status = rclcpp::spin_until_future_complete(node, future_result, time_out); - - if (future_status != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s time out while getting current state.", service_name.c_str()); - return MsgState::PRIMARY_STATE_UNKNOWN; - } - - return future_result.get()->current_state.id; -} - -bool all_nodes_are_unconfigured( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) -{ - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_UNCONFIGURED; - }); -} - -bool all_nodes_are_inactive( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) -{ - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_INACTIVE; - }); -} - -bool all_nodes_are_active( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) -{ - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return state_of(s, node, 10s) == MsgState::PRIMARY_STATE_ACTIVE; - }); -} - -bool change_state( - std::string target_node_name, rclcpp::Node::SharedPtr node, - std::uint8_t transition, std::chrono::seconds time_out = 10s) -{ - auto request = std::make_shared(); - request->transition.id = transition; - - auto service_name = target_node_name + "/change_state"; - auto client = node->create_client(service_name); - - if (!client->wait_for_service(time_out)) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s is not avaliable.", service_name.c_str()); - return false; - } - - auto future_result = client->async_send_request(request); - auto future_status = rclcpp::spin_until_future_complete(node, future_result, time_out); - - if (future_status != rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR( - node->get_logger(), - "Service %s time out while changing current state.", service_name.c_str()); - return false; - } - - return future_result.get()->success; -} - -bool configure_all_nodes( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) -{ - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return change_state(s, node, MsgTransition::TRANSITION_CONFIGURE, 10s); - }); -} - -bool activate_all_nodes( - rclcpp::Node::SharedPtr node, - const std::vector & target_node_names) -{ - return std::all_of( - target_node_names.begin(), target_node_names.end(), - [&](std::string s) { - return change_state(s, node, MsgTransition::TRANSITION_ACTIVATE, 10s); - }); -} - - -int main(int argc, char * argv[]) -{ - // Force flush of the stdout buffer. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - auto node = rclcpp::Node::make_shared("lifecycle_node_manager"); - - node->declare_parameter("components", std::vector()); - auto components = node->get_parameter("components").get_value>(); - - if (components.size() == 0) { - RCLCPP_ERROR(node->get_logger(), "param 'components' has no value."); - rclcpp::shutdown(); - } - - if (!all_nodes_are_unconfigured(node, components)) { - RCLCPP_ERROR(node->get_logger(), "Failed to launch nodes."); - rclcpp::shutdown(); - } else { - RCLCPP_INFO(node->get_logger(), "Launched all nodes."); - } - - if (!configure_all_nodes(node, components)) { - RCLCPP_ERROR(node->get_logger(), "Failed to configure nodes."); - rclcpp::shutdown(); - } else { - RCLCPP_INFO(node->get_logger(), "Configured all nodes."); - } - - if (!activate_all_nodes(node, components)) { - RCLCPP_ERROR(node->get_logger(), "Failed to activate nodes."); - rclcpp::shutdown(); - } else { - RCLCPP_INFO(node->get_logger(), "Activated all nodes."); - } - - while (rclcpp::ok()) { - rclcpp::sleep_for(10s); - if (all_nodes_are_active(node, components)) { - RCLCPP_INFO(node->get_logger(), "All nodes are active."); - } else { - // all node shutdown - RCLCPP_ERROR(node->get_logger(), "Any node is not active."); - } - } - - rclcpp::shutdown(); - - return 0; -} diff --git a/raspimouse_gazebo/launch/raspimouse_with_emptyworld.launch.py b/raspimouse_gazebo/launch/raspimouse_with_emptyworld.launch.py index 3270b81..d7ba63e 100644 --- a/raspimouse_gazebo/launch/raspimouse_with_emptyworld.launch.py +++ b/raspimouse_gazebo/launch/raspimouse_with_emptyworld.launch.py @@ -123,15 +123,6 @@ def generate_launch_description(): output='screen', ) - manager = Node( - name='manager', - package='raspimouse_fake', - executable='lifecycle_node_manager', - output='screen', - parameters=[{'components': ['raspimouse']}] - - ) - return LaunchDescription([ SetParameter(name='use_sim_time', value=True), declare_arg_lidar, @@ -143,6 +134,5 @@ def generate_launch_description(): spawn_diff_drive_controller, rviz, bridge, - container, - manager + container ])