merge with main

Signed-off-by: stevedan <[email protected]>

nav2_bringup/params/nav2_params.yaml

@@ -407,11 +407,12 @@ docking_server:
       filter_coef: 0.1
 
     # Dock instances
-    docks: ['home_dock']  # Input your docks here
-    home_dock:
-      type: 'simple_charging_dock'
-      frame: map
-      pose: [0.0, 0.0, 0.0]
+    # The following example illustrates configuring dock instances.
+    # docks: ['home_dock']  # Input your docks here
+    # home_dock:
+    #   type: 'simple_charging_dock'
+    #   frame: map
+    #   pose: [0.0, 0.0, 0.0]
 
     controller:
       k_phi: 3.0

nav2_docking/opennav_docking/src/dock_database.cpp

@@ -34,21 +34,32 @@ bool DockDatabase::initialize(
   node_ = parent;
   auto node = node_.lock();
 
-  if (getDockPlugins(node, tf) && getDockInstances(node)) {
-    RCLCPP_INFO(
+  if (!getDockPlugins(node, tf)) {
+    RCLCPP_ERROR(
       node->get_logger(),
-      "Docking Server has %u dock types and %u dock instances available.",
-      this->plugin_size(), this->instance_size());
-    return true;
+      "An error occurred while getting the dock plugins!");
+    return false;
+  }
+
+  if (!getDockInstances(node)) {
+    RCLCPP_ERROR(
+      node->get_logger(),
+      "An error occurred while getting the dock instances!");
+    return false;
   }
 
+  RCLCPP_INFO(
+    node->get_logger(),
+    "Docking Server has %u dock types and %u dock instances available.",
+    this->plugin_size(), this->instance_size());
+
   reload_db_service_ = node->create_service<nav2_msgs::srv::ReloadDockDatabase>(
     "~/reload_database",
     std::bind(
       &DockDatabase::reloadDbCb, this,
       std::placeholders::_1, std::placeholders::_2));
 
-  return false;
+  return true;
 }
 
 void DockDatabase::activate()
@@ -71,10 +82,14 @@ void DockDatabase::reloadDbCb(
   const std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Request> request,
   std::shared_ptr<nav2_msgs::srv::ReloadDockDatabase::Response> response)
 {
+  auto node = node_.lock();
   DockMap dock_instances;
-  if (utils::parseDockFile(request->filepath, node_.lock(), dock_instances)) {
+  if (utils::parseDockFile(request->filepath, node, dock_instances)) {
     dock_instances_ = dock_instances;
     response->success = true;
+    RCLCPP_INFO(
+      node->get_logger(),
+      "Dock database reloaded from file %s.", request->filepath.c_str());
     return;
   }
   response->success = false;
@@ -194,10 +209,12 @@ bool DockDatabase::getDockInstances(const rclcpp_lifecycle::LifecycleNode::Share
     return utils::parseDockParams(docks_param, node, dock_instances_);
   }
 
-  RCLCPP_ERROR(
+  RCLCPP_WARN(
     node->get_logger(),
-    "Dock database filepath nor dock parameters set. Unable to perform docking actions.");
-  return false;
+    "Dock database filepath nor dock parameters set. "
+    "Docking actions can only be executed specifying the dock pose via the action request. "
+    "Or update the dock database via the reload_database service.");
+  return true;
 }
 
 unsigned int DockDatabase::plugin_size() const

nav2_docking/opennav_docking/test/test_dock_database.cpp

@@ -102,8 +102,11 @@ TEST(DatabaseTests, findTests)
 TEST(DatabaseTests, reloadDbService)
 {
   auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("test");
-  std::vector<std::string> plugins{"dockv1", "dockv2"};
+  std::vector<std::string> plugins{"dockv1"};
   node->declare_parameter("dock_plugins", rclcpp::ParameterValue(plugins));
+  node->declare_parameter(
+    "dockv1.plugin",
+    rclcpp::ParameterValue("opennav_docking::SimpleChargingDock"));
   opennav_docking::DockDatabase db;
   db.initialize(node, nullptr);
 

nav2_simple_commander/nav2_simple_commander/utils.py

@@ -0,0 +1,55 @@
+#! /usr/bin/env python3
+# Copyright 2024 Open Navigation LLC
+# Copyright 2024 Stevedan Ogochukwu Omodolor Omodia
+#
+# 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.
+
+"""General utility functions."""
+
+import os
+import signal
+import subprocess
+
+
+def find_os_processes(name):
+    """Find all the processes that are running gz sim."""
+    ps_output = subprocess.check_output(['ps', 'aux'], text=True)
+    ps_lines = ps_output.split('\n')
+    gz_sim_processes = []
+    for line in ps_lines:
+        if name in line:
+            columns = line.split()
+            pid = columns[1]
+            command = ' '.join(columns[10:])
+            if command.startswith(name):
+                gz_sim_processes.append((pid, command))
+    return gz_sim_processes
+
+
+def kill_process(pid):
+    """Kill a process with a given PID."""
+    try:
+        os.kill(int(pid), signal.SIGKILL)
+        print(f'Successfully killed process with PID: {pid}')
+    except Exception as e:
+        print(f'Failed to kill process with PID: {pid}. Error: {e}')
+
+
+def kill_os_processes(name):
+    """Kill all processes that are running with name."""
+    processes = find_os_processes(name)
+    if processes:
+        for pid, _ in processes:
+            kill_process(pid)
+    else:
+        print(f'No processes found starting with {name}')

nav2_system_tests/CMakeLists.txt

@@ -28,6 +28,7 @@ find_package(navigation2)
 find_package(angles REQUIRED)
 find_package(behaviortree_cpp REQUIRED)
 find_package(pluginlib REQUIRED)
+find_package(nav2_minimal_tb3_sim REQUIRED)
 
 nav2_package()
 
@@ -127,7 +128,7 @@ if(BUILD_TESTING)
   # add_subdirectory(src/behaviors/assisted_teleop)
   # add_subdirectory(src/costmap_filters)
   add_subdirectory(src/error_codes)
-  install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
+  install(DIRECTORY maps models DESTINATION share/${PROJECT_NAME})
 
 endif()
 

nav2_system_tests/models/cardboard_box.sdf

@@ -0,0 +1,9 @@
+<sdf version="1.6">
+     <model name='cardboard_box'>
+      <include>
+      <uri>
+      https://fuel.gazebosim.org/1.0/OpenRobotics/models/Cardboard box
+      </uri>
+      </include>
+    </model>
+</sdf>

nav2_system_tests/package.xml

@@ -30,6 +30,7 @@
   <build_depend>launch_ros</build_depend>
   <build_depend>launch_testing</build_depend>
   <build_depend>nav2_planner</build_depend>
+  <build_depend>nav2_minimal_tb3_sim</build_depend>
 
   <exec_depend>launch_ros</exec_depend>
   <exec_depend>launch_testing</exec_depend>
@@ -48,6 +49,7 @@
   <exec_depend>nav2_amcl</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>tf2_geometry_msgs</exec_depend>
+  <exec_depend>nav2_minimal_tb3_sim</exec_depend>
   <!-- <exec_depend>gazebo_ros_pkgs</exec_depend> Remove/Replace after https://github.com/ros-navigation/navigation2/pull/3634 -->
   <exec_depend>navigation2</exec_depend>
   <exec_depend>lcov</exec_depend>

nav2_system_tests/src/localization/test_localization_launch.py

@@ -15,6 +15,7 @@
 # limitations under the License.
 
 import os
+from pathlib import Path
 import sys
 
 from ament_index_python.packages import get_package_share_directory
@@ -26,6 +27,8 @@
 import launch_ros.actions
 from launch_testing.legacy import LaunchTestService
 
+from nav2_simple_commander.utils import kill_os_processes
+
 
 def main(argv=sys.argv[1:]):
     testExecutable = os.getenv('TEST_EXECUTABLE')
@@ -36,11 +39,19 @@ def main(argv=sys.argv[1:]):
     world_sdf_xacro = os.path.join(sim_dir, 'worlds', 'tb3_sandbox.sdf.xacro')
     robot_sdf = os.path.join(sim_dir, 'urdf', 'gz_waffle.sdf')
 
+    urdf = os.path.join(sim_dir, 'urdf', 'turtlebot3_waffle.urdf')
+    with open(urdf, 'r') as infp:
+        robot_description = infp.read()
+
     map_yaml_file = os.path.join(nav2_bringup_dir, 'maps', 'tb3_sandbox.yaml')
 
     set_env_vars_resources = AppendEnvironmentVariable(
         'GZ_SIM_RESOURCE_PATH', os.path.join(sim_dir, 'models')
     )
+    set_env_vars_resources2 = AppendEnvironmentVariable(
+        'GZ_SIM_RESOURCE_PATH',
+        str(Path(os.path.join(sim_dir)).parent.resolve())
+    )
 
     start_gazebo_server = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
@@ -64,18 +75,14 @@ def main(argv=sys.argv[1:]):
             'yaw': '0.0',
         }.items(),
     )
-
-    link_footprint = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
-        output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_footprint', 'base_link'],
-    )
-    footprint_scan = launch_ros.actions.Node(
-        package='tf2_ros',
-        executable='static_transform_publisher',
+    run_robot_state_publisher = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
         output='screen',
-        arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan'],
+        parameters=[
+            {'use_sim_time': True, 'robot_description': robot_description}
+        ],
     )
     run_map_server = launch_ros.actions.Node(
         package='nav2_map_server',
@@ -97,10 +104,10 @@ def main(argv=sys.argv[1:]):
     ld = LaunchDescription(
         [
             set_env_vars_resources,
+            set_env_vars_resources2,
             start_gazebo_server,
             spawn_robot,
-            link_footprint,
-            footprint_scan,
+            run_robot_state_publisher,
             run_map_server,
             run_amcl,
             run_lifecycle_manager,
@@ -116,11 +123,7 @@ def main(argv=sys.argv[1:]):
     ls = LaunchService(argv=argv)
     ls.include_launch_description(ld)
     return_code = lts.run(ls)
-    # (TODO ) This is a workaround to kill the gz server after the test
-    # We noticed that the gz server is not killed after the test
-    # and it is still running in the background. This affects
-    # the next test run. This is a temporary fix until we find
-    # a better way of killing the gz server after the test.
+    kill_os_processes('gz sim')
     return return_code
 
 

nav2_system_tests/src/system/CMakeLists.txt

@@ -5,9 +5,6 @@ ament_add_test(test_bt_navigator
   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_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=True
@@ -22,9 +19,6 @@ ament_add_test(test_bt_navigator_with_wrong_init_pose
   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_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=True
@@ -39,9 +33,6 @@ ament_add_test(test_bt_navigator_with_dijkstra
   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_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -56,9 +47,6 @@ ament_add_test(test_bt_navigator_2
   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_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -68,14 +56,12 @@ ament_add_test(test_bt_navigator_2
 
 ament_add_test(test_dynamic_obstacle
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_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_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
     TESTER=nav_to_pose_tester_node.py
     ASTAR=False
@@ -85,14 +71,12 @@ ament_add_test(test_dynamic_obstacle
 
 ament_add_test(test_nav_through_poses
   GENERATE_RESULT_FOR_RETURN_CODE_ZERO
-  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_launch.py"
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_system_with_obstacle_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_obstacle.world
-    GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
+    ADD_OBSTACLE=True
     BT_NAVIGATOR_XML=navigate_through_poses_w_replanning_and_recovery.xml
     TESTER=nav_through_poses_tester_node.py
     ASTAR=False