From 8501a84d770e5551832f5ee2707668851f467e8a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 4 Aug 2020 17:24:48 -0700 Subject: [PATCH 1/5] Bunches of random new tests (#1909) * trying to get A* to work again * make flake 8 happy * adding cancel and preempt test * planner tests use A* * adding A* note * test with topic --- nav2_planner/src/planner_server.cpp | 5 +-- .../src/planning/planner_tester.cpp | 4 +++ nav2_system_tests/src/system/CMakeLists.txt | 7 ++-- .../src/system/test_system_launch.py | 3 +- nav2_system_tests/src/system/tester_node.py | 8 ++--- .../src/waypoint_follower/tester.py | 36 ++++++++++++++----- .../waypoint_follower.hpp | 1 + .../src/waypoint_follower.cpp | 5 +-- 8 files changed, 46 insertions(+), 23 deletions(-) diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index fc8b4ab9b61..9224290fa6b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -66,10 +66,7 @@ PlannerServer::PlannerServer() PlannerServer::~PlannerServer() { RCLCPP_INFO(get_logger(), "Destroying"); - PlannerMap::iterator it; - for (it = planners_.begin(); it != planners_.end(); ++it) { - it->second.reset(); - } + planners_.clear(); } nav2_util::CallbackReturn diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 534b8237301..4c65a5a7b2a 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -67,6 +67,10 @@ void PlannerTester::activate() // The navfn wrapper auto state = rclcpp_lifecycle::State(); planner_tester_ = std::make_shared(); + planner_tester_->declare_parameter( + "GridBased.use_astar", rclcpp::ParameterValue(true)); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/system/CMakeLists.txt b/nav2_system_tests/src/system/CMakeLists.txt index 09017bfc6ca..a7ae2aa43fa 100644 --- a/nav2_system_tests/src/system/CMakeLists.txt +++ b/nav2_system_tests/src/system/CMakeLists.txt @@ -1,3 +1,6 @@ +# NOTE: The ASTAR=True does not work currently due to remapping not functioning +# All set to false, A* testing of NavFn happens in the planning test portion + ament_add_test(test_bt_navigator GENERATE_RESULT_FOR_RETURN_CODE_ZERO COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py" @@ -9,7 +12,7 @@ ament_add_test(test_bt_navigator TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) ament_add_test(test_bt_navigator_with_dijkstra @@ -37,7 +40,7 @@ ament_add_test(test_dynamic_obstacle TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_obstacle.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml - ASTAR=True + ASTAR=False ) # ament_add_test(test_multi_robot diff --git a/nav2_system_tests/src/system/test_system_launch.py b/nav2_system_tests/src/system/test_system_launch.py index dde6618cdc7..92ea252eba0 100755 --- a/nav2_system_tests/src/system/test_system_launch.py +++ b/nav2_system_tests/src/system/test_system_launch.py @@ -41,7 +41,8 @@ def generate_launch_description(): params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') # Replace the `use_astar` setting on the params file - param_substitutions = {'GridBased.use_astar': os.getenv('ASTAR')} + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': os.getenv('ASTAR')} configured_params = RewrittenYaml( source_file=params_file, root_key='', diff --git a/nav2_system_tests/src/system/tester_node.py b/nav2_system_tests/src/system/tester_node.py index a66e2f7b89d..83e9df9dc9f 100755 --- a/nav2_system_tests/src/system/tester_node.py +++ b/nav2_system_tests/src/system/tester_node.py @@ -228,12 +228,8 @@ def run_all_tests(robot_tester): robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() - # TODO(orduno) Test sending the navigation request through the topic interface. - # Need to update tester to receive multiple goal poses. - # Need to fix bug with shutting down while bt_navigator - # is still running. - # if (result): - # result = test_RobotMovesToGoal(robot_tester) + if (result): + result = test_RobotMovesToGoal(robot_tester) # Add more tests here if desired diff --git a/nav2_system_tests/src/waypoint_follower/tester.py b/nav2_system_tests/src/waypoint_follower/tester.py index 31bd9effbf2..97ad511675f 100755 --- a/nav2_system_tests/src/waypoint_follower/tester.py +++ b/nav2_system_tests/src/waypoint_follower/tester.py @@ -37,6 +37,7 @@ def __init__(self): self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, 'initialpose', 10) self.initial_pose_received = False + self.goal_handle = None pose_qos = QoSProfile( durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, @@ -71,7 +72,7 @@ def setWaypoints(self, waypoints): msg.pose.orientation.w = 1.0 self.waypoints.append(msg) - def run(self): + def run(self, block): if not self.waypoints: rclpy.error_msg('Did not set valid waypoints before running test!') return False @@ -86,16 +87,19 @@ def run(self): send_goal_future = self.action_client.send_goal_async(action_request) try: rclpy.spin_until_future_complete(self, send_goal_future) - goal_handle = send_goal_future.result() + self.goal_handle = send_goal_future.result() except Exception as e: self.error_msg('Service call failed %r' % (e,)) - if not goal_handle.accepted: + if not self.goal_handle.accepted: self.error_msg('Goal rejected') return False self.info_msg('Goal accepted') - get_result_future = goal_handle.get_result_async() + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() self.info_msg("Waiting for 'FollowWaypoints' action to complete") try: @@ -148,14 +152,18 @@ def shutdown(self): except Exception as e: self.error_msg('Service call failed %r' % (e,)) + def cancel_goal(self): + cancel_future = self.goal_handle.cancel_goal_async() + rclpy.spin_until_future_complete(self, cancel_future) + def info_msg(self, msg: str): - self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + self.get_logger().info(msg) def warn_msg(self, msg: str): - self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + self.get_logger().warn(msg) def error_msg(self, msg: str): - self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + self.get_logger().error(msg) def main(argv=sys.argv[1:]): @@ -179,7 +187,19 @@ def main(argv=sys.argv[1:]): test.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(test, timeout_sec=1.0) # wait for poseCallback - result = test.run() + result = test.run(True) + + # preempt with new point + test.setWaypoints([starting_pose]) + result = test.run(False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False) + + # cancel + time.sleep(2) + test.cancel_goal() + test.shutdown() test.info_msg('Done Shutting Down.') diff --git a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp index b24147fb9b8..83ffa14f69e 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -115,6 +115,7 @@ class WaypointFollower : public nav2_util::LifecycleNode std::unique_ptr action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::Node::SharedPtr client_node_; + std::shared_future::SharedPtr> future_goal_handle_; bool stop_on_failure_; ActionStatus current_goal_status_; int loop_rate_; diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index d216c82b47a..7f941e490e1 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -132,7 +132,8 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action if (action_server_->is_cancel_requested()) { - RCLCPP_INFO(get_logger(), "Cancelling action."); + auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); + rclcpp::spin_until_future_complete(client_node_, cancel_future); action_server_->terminate_all(); return; } @@ -156,7 +157,7 @@ WaypointFollower::followWaypoints() std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); send_goal_options.goal_response_callback = std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); - auto future_goal_handle = + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_ = ActionStatus::PROCESSING; } From 87188c668038b19fb5d35bd66ef344ffd2583d56 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 4 Aug 2020 20:04:42 -0700 Subject: [PATCH 2/5] Adding failure to navigate test (#1912) * failures tests * adding copyrights * cancel test in recoveries --- nav2_bt_navigator/src/bt_navigator.cpp | 3 - nav2_controller/src/nav2_controller.cpp | 9 +- nav2_planner/src/planner_server.cpp | 9 +- nav2_system_tests/CMakeLists.txt | 1 + .../src/planning/planner_tester.cpp | 2 + .../wait/test_wait_recovery_node.cpp | 10 +- .../recoveries/wait/wait_recovery_tester.cpp | 77 +++++ .../recoveries/wait/wait_recovery_tester.hpp | 2 + .../src/system_failure/CMakeLists.txt | 12 + .../src/system_failure/README.md | 3 + .../test_system_failure_launch.py | 106 ++++++ .../src/system_failure/tester_node.py | 307 ++++++++++++++++++ 12 files changed, 522 insertions(+), 19 deletions(-) create mode 100644 nav2_system_tests/src/system_failure/CMakeLists.txt create mode 100644 nav2_system_tests/src/system_failure/README.md create mode 100755 nav2_system_tests/src/system_failure/test_system_failure_launch.py create mode 100755 nav2_system_tests/src/system_failure/tester_node.py diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 64e076ae621..863d2635b3c 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -312,9 +312,6 @@ BtNavigator::navigateToPose() RCLCPP_INFO(get_logger(), "Navigation canceled"); action_server_->terminate_all(); break; - - default: - throw std::logic_error("Invalid status return from BT"); } } diff --git a/nav2_controller/src/nav2_controller.cpp b/nav2_controller/src/nav2_controller.cpp index 3b6ba3971fb..7d91e0128fd 100644 --- a/nav2_controller/src/nav2_controller.cpp +++ b/nav2_controller/src/nav2_controller.cpp @@ -278,13 +278,8 @@ void ControllerServer::computeControl() rclcpp::Rate loop_rate(controller_frequency_); while (rclcpp::ok()) { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 9224290fa6b..24f104f6f7e 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -211,13 +211,8 @@ PlannerServer::computePlan() auto result = std::make_shared(); try { - if (action_server_ == nullptr) { - RCLCPP_DEBUG(get_logger(), "Action server unavailable. Stopping."); - return; - } - - if (!action_server_->is_server_active()) { - RCLCPP_DEBUG(get_logger(), "Action server is inactive. Stopping."); + if (action_server_ == nullptr || !action_server_->is_server_active()) { + RCLCPP_DEBUG(get_logger(), "Action server unavailable or inactive. Stopping."); return; } diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 9eae0646f2f..8b4178c6716 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -55,6 +55,7 @@ if(BUILD_TESTING) add_subdirectory(src/planning) add_subdirectory(src/localization) add_subdirectory(src/system) + add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) add_subdirectory(src/recoveries/spin) diff --git a/nav2_system_tests/src/planning/planner_tester.cpp b/nav2_system_tests/src/planning/planner_tester.cpp index 4c65a5a7b2a..1367735aa26 100644 --- a/nav2_system_tests/src/planning/planner_tester.cpp +++ b/nav2_system_tests/src/planning/planner_tester.cpp @@ -71,6 +71,8 @@ void PlannerTester::activate() "GridBased.use_astar", rclcpp::ParameterValue(true)); planner_tester_->set_parameter( rclcpp::Parameter(std::string("GridBased.use_astar"), rclcpp::ParameterValue(true))); + planner_tester_->set_parameter( + rclcpp::Parameter(std::string("expected_planner_frequency"), rclcpp::ParameterValue(-1.0))); planner_tester_->onConfigure(state); publishRobotTransform(); map_pub_ = this->create_publisher("map", 1); diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp index e14ca5c7dcf..39ee9871d29 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp @@ -62,11 +62,16 @@ WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) { float wait_time = std::get<0>(GetParam()); + float cancel = std::get<1>(GetParam()); bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || wait_recovery_tester->recoveryTest(wait_time); + if (cancel == 1.0) { + success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + } else { + success = success || wait_recovery_tester->recoveryTest(wait_time); + } if (success) { break; } @@ -81,7 +86,8 @@ INSTANTIATE_TEST_CASE_P( ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), - std::make_tuple(5.0, 0.0)), + std::make_tuple(5.0, 0.0), + std::make_tuple(10.0, 1.0)), testNameGenerator); int main(int argc, char ** argv) diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp index eb980e9a9d1..7112289b5af 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp @@ -172,6 +172,83 @@ bool WaitRecoveryTester::recoveryTest( return true; } +bool WaitRecoveryTester::recoveryTestCancel( + const float wait_time) +{ + if (!is_active_) { + RCLCPP_ERROR(node_->get_logger(), "Not activated"); + return false; + } + + // Sleep to let recovery server be ready for serving in multiple runs + std::this_thread::sleep_for(5s); + + auto start_time = node_->now(); + auto goal_msg = Wait::Goal(); + goal_msg.time = rclcpp::Duration(wait_time, 0.0); + + RCLCPP_INFO(this->node_->get_logger(), "Sending goal"); + + auto goal_handle_future = client_ptr_->async_send_goal(goal_msg); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed :("); + return false; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return false; + } + + // Wait for the server to be done with the goal + auto result_future = client_ptr_->async_cancel_all_goals(); + + RCLCPP_INFO(node_->get_logger(), "Waiting for cancellation"); + if (rclcpp::spin_until_future_complete(node_, result_future) != + rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(node_->get_logger(), "get cancel result call failed :("); + return false; + } + + auto status = goal_handle_future.get()->get_status(); + + switch (status) { + case rclcpp_action::GoalStatus::STATUS_SUCCEEDED: RCLCPP_ERROR( + node_->get_logger(), + "Goal succeeded"); + return false; + case rclcpp_action::GoalStatus::STATUS_ABORTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal was aborted"); + return false; + case rclcpp_action::GoalStatus::STATUS_CANCELED: RCLCPP_INFO( + node_->get_logger(), + "Goal was canceled"); + return true; + case rclcpp_action::GoalStatus::STATUS_CANCELING: RCLCPP_INFO( + node_->get_logger(), + "Goal is cancelling"); + return true; + case rclcpp_action::GoalStatus::STATUS_EXECUTING: RCLCPP_ERROR( + node_->get_logger(), + "Goal is executing"); + return false; + case rclcpp_action::GoalStatus::STATUS_ACCEPTED: RCLCPP_ERROR( + node_->get_logger(), + "Goal is processing"); + return false; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); + return false; + } + + return false; +} + void WaitRecoveryTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp index 5daef5025b6..4dfb90c6626 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp @@ -52,6 +52,8 @@ class WaitRecoveryTester bool recoveryTest( float time); + bool recoveryTestCancel(float time); + void activate(); void deactivate(); diff --git a/nav2_system_tests/src/system_failure/CMakeLists.txt b/nav2_system_tests/src/system_failure/CMakeLists.txt new file mode 100644 index 00000000000..885234adc21 --- /dev/null +++ b/nav2_system_tests/src/system_failure/CMakeLists.txt @@ -0,0 +1,12 @@ +ament_add_test(test_failure_navigator + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_failure_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/system_failure/README.md b/nav2_system_tests/src/system_failure/README.md new file mode 100644 index 00000000000..4afd4eaac34 --- /dev/null +++ b/nav2_system_tests/src/system_failure/README.md @@ -0,0 +1,3 @@ +# Navigation2 System Tests - Failure + +High level system failures tests diff --git a/nav2_system_tests/src/system_failure/test_system_failure_launch.py b/nav2_system_tests/src/system_failure/test_system_failure_launch.py new file mode 100755 index 00000000000..c41e78f3048 --- /dev/null +++ b/nav2_system_tests/src/system_failure/test_system_failure_launch.py @@ -0,0 +1,106 @@ +#!/usr/bin/env python3 + +# Copyright (c) 2018 Intel Corporation +# Copyright (c) 2020 Samsung Research America +# +# 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 +import sys + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch import LaunchService +from launch.actions import ExecuteProcess, IncludeLaunchDescription, SetEnvironmentVariable +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch_ros.actions import Node +from launch_testing.legacy import LaunchTestService + +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + map_yaml_file = os.getenv('TEST_MAP') + world = os.getenv('TEST_WORLD') + + bt_navigator_xml = os.path.join(get_package_share_directory('nav2_bt_navigator'), + 'behavior_trees', + os.getenv('BT_NAVIGATOR_XML')) + + bringup_dir = get_package_share_directory('nav2_bringup') + params_file = os.path.join(bringup_dir, 'params', 'nav2_params.yaml') + + # Replace the `use_astar` setting on the params file + param_substitutions = { + 'planner_server.ros__parameters.GridBased.use_astar': 'False'} + configured_params = RewrittenYaml( + source_file=params_file, + root_key='', + param_rewrites=param_substitutions, + convert_types=True) + + return LaunchDescription([ + SetEnvironmentVariable('RCUTILS_CONSOLE_STDOUT_LINE_BUFFERED', '1'), + + # Launch gazebo server for simulation + ExecuteProcess( + cmd=['gzserver', '-s', 'libgazebo_ros_init.so', + '--minimal_comms', world], + output='screen'), + + # TODO(orduno) Launch the robot state publisher instead + # using a local copy of TB3 urdf file + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link']), + + Node( + package='tf2_ros', + executable='static_transform_publisher', + output='screen', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), + + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, 'launch', 'bringup_launch.py')), + launch_arguments={'namespace': '', + 'use_namespace': 'False', + 'map': map_yaml_file, + 'use_sim_time': 'True', + 'params_file': configured_params, + 'bt_xml_file': bt_navigator_xml, + 'autostart': 'True'}.items()), + ]) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv('TEST_DIR'), 'tester_node.py'), + '-r', '-2.0', '-0.5', '100.0', '100.0'], + name='tester_node', + output='screen') + + lts = LaunchTestService() + lts.add_test_action(ld, test1_action) + ls = LaunchService(argv=argv) + ls.include_launch_description(ld) + return lts.run(ls) + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/nav2_system_tests/src/system_failure/tester_node.py b/nav2_system_tests/src/system_failure/tester_node.py new file mode 100755 index 00000000000..eac29bc6540 --- /dev/null +++ b/nav2_system_tests/src/system_failure/tester_node.py @@ -0,0 +1,307 @@ +#! /usr/bin/env python3 +# Copyright 2018 Intel Corporation. +# Copyright 2020 Samsung Research America +# +# 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 argparse +import math +import sys +import time +from typing import Optional + +from action_msgs.msg import GoalStatus +from geometry_msgs.msg import Pose +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import PoseWithCovarianceStamped +from lifecycle_msgs.srv import GetState +from nav2_msgs.action import NavigateToPose +from nav2_msgs.srv import ManageLifecycleNodes + +import rclpy + +from rclpy.action import ActionClient +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSReliabilityPolicy +from rclpy.qos import QoSProfile + + +class NavTester(Node): + + def __init__( + self, + initial_pose: Pose, + goal_pose: Pose, + namespace: str = '' + ): + super().__init__(node_name='nav2_tester', namespace=namespace) + self.initial_pose_pub = self.create_publisher(PoseWithCovarianceStamped, + 'initialpose', 10) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) + + pose_qos = QoSProfile( + durability=QoSDurabilityPolicy.RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL, + reliability=QoSReliabilityPolicy.RMW_QOS_POLICY_RELIABILITY_RELIABLE, + history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, + depth=1) + + self.model_pose_sub = self.create_subscription(PoseWithCovarianceStamped, + 'amcl_pose', self.poseCallback, pose_qos) + self.initial_pose_received = False + self.initial_pose = initial_pose + self.goal_pose = goal_pose + self.action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') + + def info_msg(self, msg: str): + self.get_logger().info('\033[1;37;44m' + msg + '\033[0m') + + def warn_msg(self, msg: str): + self.get_logger().warn('\033[1;37;43m' + msg + '\033[0m') + + def error_msg(self, msg: str): + self.get_logger().error('\033[1;37;41m' + msg + '\033[0m') + + def setInitialPose(self): + msg = PoseWithCovarianceStamped() + msg.pose.pose = self.initial_pose + msg.header.frame_id = 'map' + self.info_msg('Publishing Initial Pose') + self.initial_pose_pub.publish(msg) + self.currentPose = self.initial_pose + + def getStampedPoseMsg(self, pose: Pose): + msg = PoseStamped() + msg.header.frame_id = 'map' + msg.pose = pose + return msg + + def publishGoalPose(self, goal_pose: Optional[Pose] = None): + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + self.goal_pub.publish(self.getStampedPoseMsg(self.goal_pose)) + + def runNavigateAction(self, goal_pose: Optional[Pose] = None): + # Sends a `NavToPose` action request and waits for completion + self.info_msg("Waiting for 'NavigateToPose' action server") + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg("'NavigateToPose' action server not available, waiting...") + + self.goal_pose = goal_pose if goal_pose is not None else self.goal_pose + goal_msg = NavigateToPose.Goal() + goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) + + self.info_msg('Sending goal request...') + send_goal_future = self.action_client.send_goal_async(goal_msg) + + rclpy.spin_until_future_complete(self, send_goal_future) + goal_handle = send_goal_future.result() + + if not goal_handle.accepted: + self.error_msg('Goal rejected') + return False + + self.info_msg('Goal accepted') + get_result_future = goal_handle.get_result_async() + + self.info_msg("Waiting for 'NavigateToPose' action to complete") + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + if status != GoalStatus.STATUS_ABORTED: + self.info_msg('Goal failed with status code: {0}'.format(status)) + return False + + self.info_msg('Goal failed, as expected!') + return True + + def poseCallback(self, msg): + self.info_msg('Received amcl_pose') + self.current_pose = msg.pose.pose + self.initial_pose_received = True + + def reachesGoal(self, timeout, distance): + goalReached = False + start_time = time.time() + + while not goalReached: + rclpy.spin_once(self, timeout_sec=1) + if self.distanceFromGoal() < distance: + goalReached = True + self.info_msg('*** GOAL REACHED ***') + return True + elif timeout is not None: + if (time.time() - start_time) > timeout: + self.error_msg('Robot timed out reaching its goal!') + return False + + def distanceFromGoal(self): + d_x = self.current_pose.position.x - self.goal_pose.position.x + d_y = self.current_pose.position.y - self.goal_pose.position.y + distance = math.sqrt(d_x * d_x + d_y * d_y) + self.info_msg('Distance from goal is: ' + str(distance)) + return distance + + def wait_for_node_active(self, node_name: str): + # Waits for the node within the tester namespace to become active + self.info_msg('Waiting for ' + node_name + ' to become active') + node_service = node_name + '/get_state' + state_client = self.create_client(GetState, node_service) + while not state_client.wait_for_service(timeout_sec=1.0): + self.info_msg(node_service + ' service not available, waiting...') + req = GetState.Request() # empty request + state = 'UNKNOWN' + while (state != 'active'): + self.info_msg('Getting ' + node_name + ' state...') + future = state_client.call_async(req) + rclpy.spin_until_future_complete(self, future) + if future.result() is not None: + state = future.result().current_state.label + self.info_msg('Result of get_state: %s' % state) + else: + self.error_msg('Exception while calling service: %r' % future.exception()) + time.sleep(5) + + def shutdown(self): + self.info_msg('Shutting down') + self.action_client.destroy() + + transition_service = 'lifecycle_manager_navigation/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down navigation lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down navigation lifecycle manager complete.') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + transition_service = 'lifecycle_manager_localization/manage_nodes' + mgr_client = self.create_client(ManageLifecycleNodes, transition_service) + while not mgr_client.wait_for_service(timeout_sec=1.0): + self.info_msg(transition_service + ' service not available, waiting...') + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + self.info_msg('Shutting down localization lifecycle manager...') + rclpy.spin_until_future_complete(self, future) + future.result() + self.info_msg('Shutting down localization lifecycle manager complete') + except Exception as e: + self.error_msg('Service call failed %r' % (e,)) + + def wait_for_initial_pose(self): + self.initial_pose_received = False + while not self.initial_pose_received: + self.info_msg('Setting initial pose') + self.setInitialPose() + self.info_msg('Waiting for amcl_pose to be received') + rclpy.spin_once(self, timeout_sec=1) + + +def run_all_tests(robot_tester): + # set transforms to use_sim_time + result = True + if (result): + robot_tester.wait_for_node_active('amcl') + robot_tester.wait_for_initial_pose() + robot_tester.wait_for_node_active('bt_navigator') + result = robot_tester.runNavigateAction() + + # Add more tests here if desired + + if (result): + robot_tester.info_msg('Test PASSED') + else: + robot_tester.error_msg('Test FAILED') + + return result + + +def fwd_pose(x=0.0, y=0.0, z=0.01): + initial_pose = Pose() + initial_pose.position.x = x + initial_pose.position.y = y + initial_pose.position.z = z + initial_pose.orientation.x = 0.0 + initial_pose.orientation.y = 0.0 + initial_pose.orientation.z = 0.0 + initial_pose.orientation.w = 1.0 + return initial_pose + + +def get_testers(args): + testers = [] + + if args.robot: + # Requested tester for one robot + init_x, init_y, final_x, final_y = args.robot[0] + tester = NavTester( + initial_pose=fwd_pose(float(init_x), float(init_y)), + goal_pose=fwd_pose(float(final_x), float(final_y))) + tester.info_msg( + 'Starting tester, robot going from ' + init_x + ', ' + init_y + + ' to ' + final_x + ', ' + final_y + '.') + testers.append(tester) + return testers + + return testers + + +def main(argv=sys.argv[1:]): + # The robot(s) positions from the input arguments + parser = argparse.ArgumentParser(description='System-level navigation tester node') + group = parser.add_mutually_exclusive_group(required=True) + group.add_argument('-r', '--robot', action='append', nargs=4, + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.') + group.add_argument('-rs', '--robots', action='append', nargs=5, + metavar=('name', 'init_x', 'init_y', 'final_x', 'final_y'), + help="The robot's namespace and starting and final positions. " + + 'Repeating the argument for multiple robots is supported.') + + args, unknown = parser.parse_known_args() + + rclpy.init() + + # Create testers for each robot + testers = get_testers(args) + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + for tester in testers: + passed = run_all_tests(tester) + if not passed: + break + + for tester in testers: + # stop and shutdown the nav stack to exit cleanly + tester.shutdown() + + testers[0].info_msg('Done Shutting Down.') + + if not passed: + testers[0].info_msg('Exiting failed') + exit(1) + else: + testers[0].info_msg('Exiting passed') + exit(0) + + +if __name__ == '__main__': + main() From 6f4a5582a568391b13a409e54b5c2d2ae0a565c4 Mon Sep 17 00:00:00 2001 From: Matthijs den Toom Date: Wed, 5 Aug 2020 20:04:36 +0200 Subject: [PATCH 3/5] Speed is inverted when dist is negative (#1897) * Positive speed and distance are enforced in backup action node * Enforce positive inputs to backup recovery * Removed positive enforcement from backup action Co-authored-by: Matthijs den Toom --- .../nav2_behavior_tree/plugins/action/back_up_action.hpp | 2 +- nav2_behavior_tree/plugins/action/back_up_action.cpp | 5 ----- .../test/plugins/action/test_back_up_action.cpp | 4 ++-- nav2_recoveries/plugins/back_up.cpp | 9 +++++---- 4 files changed, 8 insertions(+), 12 deletions(-) diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp index 14960662169..e49dec78ab4 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/back_up_action.hpp @@ -37,7 +37,7 @@ class BackUpAction : public BtActionNode { return providedBasicPorts( { - BT::InputPort("backup_dist", -0.15, "Distance to backup"), + BT::InputPort("backup_dist", 0.15, "Distance to backup"), BT::InputPort("backup_speed", 0.025, "Speed at which to backup") }); } diff --git a/nav2_behavior_tree/plugins/action/back_up_action.cpp b/nav2_behavior_tree/plugins/action/back_up_action.cpp index 66d66cad8ef..0ab86ec7c66 100644 --- a/nav2_behavior_tree/plugins/action/back_up_action.cpp +++ b/nav2_behavior_tree/plugins/action/back_up_action.cpp @@ -31,11 +31,6 @@ BackUpAction::BackUpAction( double speed; getInput("backup_speed", speed); - // silently fix, vector direction determined by distance sign - if (speed < 0.0) { - speed *= -1.0; - } - // Populate the input message goal_.target.x = dist; goal_.target.y = 0.0; diff --git a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp index c01708d5c46..8906d03d7fc 100644 --- a/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_back_up_action.cpp @@ -114,7 +114,7 @@ TEST_F(BackUpActionTestFixture, test_ports) )"; tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); - EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), -0.15); + EXPECT_EQ(tree_->rootNode()->getInput("backup_dist"), 0.15); EXPECT_EQ(tree_->rootNode()->getInput("backup_speed"), 0.025); xml_txt = @@ -136,7 +136,7 @@ TEST_F(BackUpActionTestFixture, test_tick) R"( - + )"; diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp index 585d8a77410..fae91b998c5 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_recoveries/plugins/back_up.cpp @@ -51,8 +51,9 @@ Status BackUp::onRun(const std::shared_ptr command) "will only move in X."); } - command_x_ = command->target.x; - command_speed_ = command->speed; + // Silently ensure that both the speed and direction are positive. + command_x_ = std::fabs(command->target.x); + command_speed_ = std::fabs(command->speed); if (!nav2_util::getCurrentPose( initial_pose_, *tf_, global_frame_, robot_base_frame_, @@ -83,7 +84,7 @@ Status BackUp::onCycleUpdate() feedback_->distance_traveled = distance; action_server_->publish_feedback(feedback_); - if (distance >= abs(command_x_)) { + if (distance >= command_x_) { stopRobot(); return Status::SUCCEEDED; } @@ -92,7 +93,7 @@ Status BackUp::onCycleUpdate() auto cmd_vel = std::make_unique(); cmd_vel->linear.y = 0.0; cmd_vel->angular.z = 0.0; - command_x_ < 0 ? cmd_vel->linear.x = -command_speed_ : cmd_vel->linear.x = command_speed_; + cmd_vel->linear.x = -command_speed_; geometry_msgs::msg::Pose2D pose2d; pose2d.x = current_pose.pose.position.x; From cf3a9f8d971d680957b121c6d4b1e948c05d48dd Mon Sep 17 00:00:00 2001 From: Daisuke Sato <43101027+daisukes@users.noreply.github.com> Date: Wed, 5 Aug 2020 14:43:01 -0400 Subject: [PATCH 4/5] Costmap lock while copying data in navfn planner (#1913) * acquire the costmap lock while copying data in navfn planner Signed-off-by: Daisuke Sato * add costmap lock to dwb local plannger Signed-off-by: Daisuke Sato --- nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp | 8 ++++++++ nav2_navfn_planner/src/navfn_planner.cpp | 4 ++++ 2 files changed, 12 insertions(+) diff --git a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp index 8c7041bcb72..245323db510 100644 --- a/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp +++ b/nav2_dwb_controller/dwb_core/src/dwb_local_planner.cpp @@ -325,6 +325,9 @@ DWBLocalPlanner::computeVelocityCommands( prepareGlobalPlan(pose, transformed_plan, goal_pose); + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + std::unique_lock lock(*(costmap->getMutex())); + for (TrajectoryCritic::Ptr critic : critics_) { if (critic->prepare(pose.pose, velocity, goal_pose.pose, transformed_plan) == false) { RCLCPP_WARN(rclcpp::get_logger("DWBLocalPlanner"), "A scoring function failed to prepare"); @@ -344,6 +347,8 @@ DWBLocalPlanner::computeVelocityCommands( critic->debrief(cmd_vel.velocity); } + lock.unlock(); + pub_->publishLocalPlan(pose.header, best.traj); pub_->publishCostGrid(costmap_ros_, critics_); @@ -355,6 +360,9 @@ DWBLocalPlanner::computeVelocityCommands( for (TrajectoryCritic::Ptr critic : critics_) { critic->debrief(empty_cmd); } + + lock.unlock(); + pub_->publishLocalPlan(pose.header, empty_traj); pub_->publishCostGrid(costmap_ros_, critics_); diff --git a/nav2_navfn_planner/src/navfn_planner.cpp b/nav2_navfn_planner/src/navfn_planner.cpp index d2914945894..898c1648d76 100644 --- a/nav2_navfn_planner/src/navfn_planner.cpp +++ b/nav2_navfn_planner/src/navfn_planner.cpp @@ -174,6 +174,8 @@ NavfnPlanner::makePlan( // clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(mx, my); + std::unique_lock lock(*(costmap_->getMutex())); + // make sure to resize the underlying array that Navfn uses planner_->setNavArr( costmap_->getSizeInCellsX(), @@ -181,6 +183,8 @@ NavfnPlanner::makePlan( planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); + lock.unlock(); + int map_start[2]; map_start[0] = mx; map_start[1] = my; From cf0d8a0545459213f68db0c5a6fdb24e901f7fcf Mon Sep 17 00:00:00 2001 From: Michael Equi <32988490+Michael-Equi@users.noreply.github.com> Date: Wed, 5 Aug 2020 14:46:53 -0400 Subject: [PATCH 5/5] Added map_topic parameters to static layer and amcl (#1910) --- doc/parameters/param_list.md | 2 ++ nav2_amcl/include/nav2_amcl/amcl_node.hpp | 1 + nav2_amcl/src/amcl_node.cpp | 7 ++++++- nav2_costmap_2d/plugins/static_layer.cpp | 10 +++++++++- 4 files changed, 18 insertions(+), 2 deletions(-) diff --git a/doc/parameters/param_list.md b/doc/parameters/param_list.md index 3bbacfa0902..1684789885f 100644 --- a/doc/parameters/param_list.md +++ b/doc/parameters/param_list.md @@ -78,6 +78,7 @@ When `plugins` parameter is not overridden, the following default plugins are lo | ``.subscribe_to_updates | false | Subscribe to static map updates after receiving first | | ``.map_subscribe_transient_local | true | QoS settings for map topic | | ``.transform_tolerance | 0.0 | TF tolerance | +| ``.map_topic | "" | Name of the map topic to subscribe to (empty means use the map_topic defined by `costmap_2d_ros`) | ## inflation_layer plugin @@ -561,6 +562,7 @@ When `recovery_plugins` parameter is not overridden, the following default plugi | z_short | 0.05 | Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand. | | always_reset_initial_pose | false | Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize | | scan_topic | scan | Topic to subscribe to in order to receive the laser scan for localization | +| map_topic | map | Topic to subscribe to in order to receive the map for localization | --- diff --git a/nav2_amcl/include/nav2_amcl/amcl_node.hpp b/nav2_amcl/include/nav2_amcl/amcl_node.hpp index 562af9f128b..dc86baaf6e1 100644 --- a/nav2_amcl/include/nav2_amcl/amcl_node.hpp +++ b/nav2_amcl/include/nav2_amcl/amcl_node.hpp @@ -249,6 +249,7 @@ class AmclNode : public nav2_util::LifecycleNode double z_short_; double z_rand_; std::string scan_topic_{"scan"}; + std::string map_topic_{"map"}; }; } // namespace nav2_amcl diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index d31adf34718..86571e81f8e 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -217,6 +217,10 @@ AmclNode::AmclNode() add_parameter( "scan_topic", rclcpp::ParameterValue("scan"), "Topic to subscribe to in order to receive the laser scan for localization"); + + add_parameter( + "map_topic", rclcpp::ParameterValue("map"), + "Topic to subscribe to in order to receive the map to localize on"); } AmclNode::~AmclNode() @@ -1102,6 +1106,7 @@ AmclNode::initParameters() get_parameter("first_map_only_", first_map_only_); get_parameter("always_reset_initial_pose", always_reset_initial_pose_); get_parameter("scan_topic", scan_topic_); + get_parameter("map_topic", map_topic_); save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate); transform_tolerance_ = tf2::durationFromSec(tmp_tol); @@ -1277,7 +1282,7 @@ AmclNode::initPubSub() std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1)); map_sub_ = create_subscription( - "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), + map_topic_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), std::bind(&AmclNode::mapReceived, this, std::placeholders::_1)); RCLCPP_INFO(get_logger(), "Subscribed to map topic."); diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 411981813f6..cb3a01f1be4 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -124,10 +124,18 @@ StaticLayer::getParameters() declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); declareParameter("map_subscribe_transient_local", rclcpp::ParameterValue(true)); declareParameter("transform_tolerance", rclcpp::ParameterValue(0.0)); + declareParameter("map_topic", rclcpp::ParameterValue("")); node_->get_parameter(name_ + "." + "enabled", enabled_); node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_); - node_->get_parameter("map_topic", map_topic_); + std::string private_map_topic, global_map_topic; + node_->get_parameter(name_ + "." + "map_topic", private_map_topic); + node_->get_parameter("map_topic", global_map_topic); + if (!private_map_topic.empty()) { + map_topic_ = private_map_topic; + } else { + map_topic_ = global_map_topic; + } node_->get_parameter( name_ + "." + "map_subscribe_transient_local", map_subscribe_transient_local_);