Skip to content

Commit

Permalink
Merge branch 'main' into feat/port_upown_tests_to_new_gazebo
Browse files Browse the repository at this point in the history
  • Loading branch information
stevedanomodolor committed Jun 20, 2024
2 parents 3e0aa4e + 9057456 commit aea0c85
Show file tree
Hide file tree
Showing 21 changed files with 488 additions and 2,250 deletions.
11 changes: 6 additions & 5 deletions nav2_bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
37 changes: 27 additions & 10 deletions nav2_docking/opennav_docking/src/dock_database.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand All @@ -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;
Expand Down Expand Up @@ -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
Expand Down
5 changes: 4 additions & 1 deletion nav2_docking/opennav_docking/test/test_dock_database.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
55 changes: 55 additions & 0 deletions nav2_simple_commander/nav2_simple_commander/utils.py
Original file line number Diff line number Diff line change
@@ -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}')
7 changes: 4 additions & 3 deletions nav2_system_tests/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand Down Expand Up @@ -113,9 +114,9 @@ if(BUILD_TESTING)

add_subdirectory(src/behavior_tree)
add_subdirectory(src/planning)
# Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
add_subdirectory(src/localization)
# add_subdirectory(src/system)
add_subdirectory(src/system)
# Uncomment after https://github.com/ros-navigation/navigation2/pull/3634
# add_subdirectory(src/system_failure)
add_subdirectory(src/updown)
# add_subdirectory(src/waypoint_follower)
Expand All @@ -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()

Expand Down
9 changes: 9 additions & 0 deletions nav2_system_tests/models/cardboard_box.sdf
Original file line number Diff line number Diff line change
@@ -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>
2 changes: 2 additions & 0 deletions nav2_system_tests/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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>
Expand Down
37 changes: 23 additions & 14 deletions nav2_system_tests/src/localization/test_localization_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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')
Expand All @@ -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(
Expand All @@ -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',
Expand All @@ -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,
Expand All @@ -115,7 +122,9 @@ def main(argv=sys.argv[1:]):
lts.add_test_action(ld, test1_action)
ls = LaunchService(argv=argv)
ls.include_launch_description(ld)
return lts.run(ls)
return_code = lts.run(ls)
kill_os_processes('gz sim')
return return_code


if __name__ == '__main__':
Expand Down
24 changes: 4 additions & 20 deletions nav2_system_tests/src/system/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand All @@ -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
Expand Down
Loading

0 comments on commit aea0c85

Please sign in to comment.