From fa165dddc4e9850a510729c1ae7317f118aafa1f Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu Date: Sun, 22 May 2022 06:10:24 +0900 Subject: [PATCH 1/4] fix test Signed-off-by: Daisuke Nishimatsu --- nav2_bringup/launch/bringup_launch.py | 2 +- nav2_bringup/launch/localization_launch.py | 2 +- nav2_bringup/launch/navigation_launch.py | 2 +- .../nav2_simple_commander/robot_navigator.py | 2 +- nav2_smac_planner/test/test_a_star.cpp | 2 +- .../src/system/nav_to_pose_tester_node.py | 153 +++++++++--------- 6 files changed, 82 insertions(+), 81 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 29c6c616ee..bd47147227 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -22,8 +22,8 @@ from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import PushRosNamespace from launch_ros.actions import Node +from launch_ros.actions import PushRosNamespace from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index b9ab0baeaf..218debd918 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -20,8 +20,8 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index f5bcc83b55..fec21d1f0b 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -20,8 +20,8 @@ from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration, PythonExpression -from launch_ros.actions import Node from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from nav2_common.launch import RewrittenYaml diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index fb8df7f2ce..1026f8f75d 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -30,8 +30,8 @@ from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes import rclpy -from rclpy.duration import Duration as rclpyDuration from rclpy.action import ActionClient +from rclpy.duration import Duration as rclpyDuration from rclpy.node import Node from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy from rclpy.qos import QoSProfile, QoSReliabilityPolicy diff --git a/nav2_smac_planner/test/test_a_star.cpp b/nav2_smac_planner/test/test_a_star.cpp index fb0caf2704..68af08df50 100644 --- a/nav2_smac_planner/test/test_a_star.cpp +++ b/nav2_smac_planner/test/test_a_star.cpp @@ -225,7 +225,7 @@ TEST(AStarTest, test_a_star_lattice) // check path is the right size and collision free EXPECT_EQ(num_it, 21); - EXPECT_EQ(path.size(), 47u); + EXPECT_EQ(path.size(), 48u); for (unsigned int i = 0; i != path.size(); i++) { EXPECT_EQ(costmapA->getCost(path[i].x, path[i].y), 0); } diff --git a/nav2_system_tests/src/system/nav_to_pose_tester_node.py b/nav2_system_tests/src/system/nav_to_pose_tester_node.py index 6fb119e980..8fd1bc6034 100755 --- a/nav2_system_tests/src/system/nav_to_pose_tester_node.py +++ b/nav2_system_tests/src/system/nav_to_pose_tester_node.py @@ -38,12 +38,13 @@ class NavTester(Node): - def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""): - super().__init__(node_name="nav2_tester", namespace=namespace) + + 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 + PoseWithCovarianceStamped, 'initialpose', 10 ) - self.goal_pub = self.create_publisher(PoseStamped, "goal_pose", 10) + self.goal_pub = self.create_publisher(PoseStamped, 'goal_pose', 10) pose_qos = QoSProfile( durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, @@ -53,33 +54,33 @@ def __init__(self, initial_pose: Pose, goal_pose: Pose, namespace: str = ""): ) self.model_pose_sub = self.create_subscription( - PoseWithCovarianceStamped, "amcl_pose", self.poseCallback, pose_qos + 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") + 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") + 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") + 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") + 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") + 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.header.frame_id = 'map' msg.pose = pose return msg @@ -97,17 +98,17 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): goal_msg = NavigateToPose.Goal() goal_msg.pose = self.getStampedPoseMsg(self.goal_pose) - self.info_msg("Sending goal request...") + 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") + self.error_msg('Goal rejected') return False - self.info_msg("Goal accepted") + self.info_msg('Goal accepted') get_result_future = goal_handle.get_result_async() future_return = True @@ -116,17 +117,17 @@ def runNavigateAction(self, goal_pose: Optional[Pose] = None): rclpy.spin_until_future_complete(self, get_result_future) status = get_result_future.result().status if status != GoalStatus.STATUS_SUCCEEDED: - self.info_msg(f"Goal failed with status code: {status}") + self.info_msg(f'Goal failed with status code: {status}') return False if not future_return: return False - self.info_msg("Goal succeeded!") + self.info_msg('Goal succeeded!') return True def poseCallback(self, msg): - self.info_msg("Received amcl_pose") + self.info_msg('Received amcl_pose') self.current_pose = msg.pose.pose self.initial_pose_received = True @@ -138,90 +139,90 @@ def reachesGoal(self, timeout, distance): rclpy.spin_once(self, timeout_sec=1) if self.distanceFromGoal() < distance: goalReached = True - self.info_msg("*** GOAL REACHED ***") + 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!") + 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(f"Distance from goal is: {distance}") + self.info_msg(f'Distance from goal is: {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(f"Waiting for {node_name} to become active") - node_service = f"{node_name}/get_state" + self.info_msg(f'Waiting for {node_name} to become active') + node_service = f'{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(f"{node_service} service not available, waiting...") + self.info_msg(f'{node_service} service not available, waiting...') req = GetState.Request() # empty request - state = "UNKNOWN" - while state != "active": - self.info_msg(f"Getting {node_name} state...") + state = 'UNKNOWN' + while state != 'active': + self.info_msg(f'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(f"Result of get_state: {state}") + self.info_msg(f'Result of get_state: {state}') else: self.error_msg( - f"Exception while calling service: {future.exception()!r}" + f'Exception while calling service: {future.exception()!r}' ) time.sleep(5) def shutdown(self): - self.info_msg("Shutting down") + self.info_msg('Shutting down') self.action_client.destroy() - transition_service = "lifecycle_manager_navigation/manage_nodes" + 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(f"{transition_service} service not available, waiting...") + self.info_msg(f'{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...") + 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.") + self.info_msg('Shutting down navigation lifecycle manager complete.') except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") - transition_service = "lifecycle_manager_localization/manage_nodes" + self.error_msg(f'Service call failed {e!r}') + 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(f"{transition_service} service not available, waiting...") + self.info_msg(f'{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...") + 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") + self.info_msg('Shutting down localization lifecycle manager complete') except Exception as e: # noqa: B902 - self.error_msg(f"Service call failed {e!r}") + self.error_msg(f'Service call failed {e!r}') def wait_for_initial_pose(self): self.initial_pose_received = False while not self.initial_pose_received: - self.info_msg("Setting initial pose") + self.info_msg('Setting initial pose') self.setInitialPose() - self.info_msg("Waiting for amcl_pose to be received") + self.info_msg('Waiting for amcl_pose to be received') rclpy.spin_once(self, timeout_sec=1) def test_RobotMovesToGoal(robot_tester): - robot_tester.info_msg("Setting goal pose") + robot_tester.info_msg('Setting goal pose') robot_tester.publishGoalPose() - robot_tester.info_msg("Waiting 60 seconds for robot to reach goal") + robot_tester.info_msg('Waiting 60 seconds for robot to reach goal') return robot_tester.reachesGoal(timeout=60, distance=0.5) @@ -229,9 +230,9 @@ 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_node_active('amcl') robot_tester.wait_for_initial_pose() - robot_tester.wait_for_node_active("bt_navigator") + robot_tester.wait_for_node_active('bt_navigator') result = robot_tester.runNavigateAction() if result: @@ -240,9 +241,9 @@ def run_all_tests(robot_tester): # Add more tests here if desired if result: - robot_tester.info_msg("Test PASSED") + robot_tester.info_msg('Test PASSED') else: - robot_tester.error_msg("Test FAILED") + robot_tester.error_msg('Test FAILED') return result @@ -270,15 +271,15 @@ def get_testers(args): goal_pose=fwd_pose(float(final_x), float(final_y)), ) tester.info_msg( - "Starting tester, robot going from " + 'Starting tester, robot going from ' + init_x - + ", " + + ', ' + init_y - + " to " + + ' to ' + final_x - + ", " + + ', ' + final_y - + "." + + '.' ) testers.append(tester) return testers @@ -292,15 +293,15 @@ def get_testers(args): goal_pose=fwd_pose(float(final_x), float(final_y)), ) tester.info_msg( - "Starting tester for " + 'Starting tester for ' + namespace - + " going from " + + ' going from ' + init_x - + ", " + + ', ' + init_y - + " to " + + ' to ' + final_x - + ", " + + ', ' + final_y ) testers.append(tester) @@ -309,9 +310,9 @@ def get_testers(args): def check_args(expect_failure: str): # Check if --expect_failure is True or False - if expect_failure != "True" and expect_failure != "False": + if expect_failure != 'True' and expect_failure != 'False': print( - "\033[1;37;41m" + " -e flag must be set to True or False only. " + "\033[0m" + '\033[1;37;41m' + ' -e flag must be set to True or False only. ' + '\033[0m' ) exit(1) else: @@ -320,25 +321,25 @@ def check_args(expect_failure: str): def main(argv=sys.argv[1:]): # The robot(s) positions from the input arguments - parser = argparse.ArgumentParser(description="System-level navigation tester node") - parser.add_argument("-e", "--expect_failure") + parser = argparse.ArgumentParser(description='System-level navigation tester node') + parser.add_argument('-e', '--expect_failure') group = parser.add_mutually_exclusive_group(required=True) group.add_argument( - "-r", - "--robot", - action="append", + '-r', + '--robot', + action='append', nargs=4, - metavar=("init_x", "init_y", "final_x", "final_y"), - help="The robot starting and final positions.", + metavar=('init_x', 'init_y', 'final_x', 'final_y'), + help='The robot starting and final positions.', ) group.add_argument( - "-rs", - "--robots", - action="append", + '-rs', + '--robots', + action='append', nargs=5, - metavar=("name", "init_x", "init_y", "final_x", "final_y"), + 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.", + + 'Repeating the argument for multiple robots is supported.', ) args, unknown = parser.parse_known_args() @@ -362,15 +363,15 @@ def main(argv=sys.argv[1:]): # stop and shutdown the nav stack to exit cleanly tester.shutdown() - testers[0].info_msg("Done Shutting Down.") + testers[0].info_msg('Done Shutting Down.') if passed != expect_failure: - testers[0].info_msg("Exiting failed") + testers[0].info_msg('Exiting failed') exit(1) else: - testers[0].info_msg("Exiting passed") + testers[0].info_msg('Exiting passed') exit(0) -if __name__ == "__main__": +if __name__ == '__main__': main() From 031dbaba687499c1bb6d44890d44b80bebaa25a6 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu Date: Sat, 28 May 2022 01:52:40 +0900 Subject: [PATCH 2/4] add bt navigator into lifecycle manager Signed-off-by: Daisuke Nishimatsu --- .../src/costmap_filters/test_keepout_launch.py | 7 ++++++- nav2_system_tests/src/costmap_filters/test_speed_launch.py | 7 ++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py index c2c7e09323..d8ad112392 100755 --- a/nav2_system_tests/src/costmap_filters/test_keepout_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_keepout_launch.py @@ -86,7 +86,12 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_filters', output='screen', - parameters=[{'node_names': ['filter_mask_server', 'costmap_filter_info_server']}, + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, {'autostart': True}]), # Nodes required for Costmap Filters configuration diff --git a/nav2_system_tests/src/costmap_filters/test_speed_launch.py b/nav2_system_tests/src/costmap_filters/test_speed_launch.py index 32f6f9a4f4..2603ea0df1 100755 --- a/nav2_system_tests/src/costmap_filters/test_speed_launch.py +++ b/nav2_system_tests/src/costmap_filters/test_speed_launch.py @@ -84,7 +84,12 @@ def generate_launch_description(): executable='lifecycle_manager', name='lifecycle_manager_filters', output='screen', - parameters=[{'node_names': ['filter_mask_server', 'costmap_filter_info_server']}, + parameters=[{ + 'node_names': + [ + 'filter_mask_server', 'costmap_filter_info_server', 'bt_navigator' + ] + }, {'autostart': True}]), # Nodes required for Costmap Filters configuration From 3190aee09ce5d76b05da4712baea0b3a7c3d74d3 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu Date: Sat, 28 May 2022 02:59:18 +0900 Subject: [PATCH 3/4] fix goal checker plugin Signed-off-by: Daisuke Nishimatsu --- nav2_system_tests/src/costmap_filters/keepout_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_global_params.yaml | 2 +- nav2_system_tests/src/costmap_filters/speed_local_params.yaml | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index c6ea7cbb77..de3722edfe 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -109,7 +109,7 @@ controller_server: min_y_velocity_threshold: 0.5 min_theta_velocity_threshold: 0.001 progress_checker_plugin: "progress_checker" - goal_checker_plugins: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index 8216c231c5..11a8f80551 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -111,7 +111,7 @@ controller_server: min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugin: "progress_checker" - goal_checker_plugins: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index ece2bb2a2e..413cff5ce1 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -111,7 +111,7 @@ controller_server: min_theta_velocity_threshold: 0.001 speed_limit_topic: "/speed_limit" progress_checker_plugin: "progress_checker" - goal_checker_plugins: "goal_checker" + goal_checker_plugins: ["goal_checker"] controller_plugins: ["FollowPath"] # Progress checker parameters From 259ab01bad5a5c8f3f1ec56b841298748c35403a Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu Date: Sat, 28 May 2022 03:01:16 +0900 Subject: [PATCH 4/4] load keepout filter param Signed-off-by: Daisuke Nishimatsu --- nav2_system_tests/src/costmap_filters/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/nav2_system_tests/src/costmap_filters/CMakeLists.txt b/nav2_system_tests/src/costmap_filters/CMakeLists.txt index e05620b366..8a48ff0859 100644 --- a/nav2_system_tests/src/costmap_filters/CMakeLists.txt +++ b/nav2_system_tests/src/costmap_filters/CMakeLists.txt @@ -8,6 +8,7 @@ ament_add_test(test_keepout_filter TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml TEST_MASK=${PROJECT_SOURCE_DIR}/maps/keepout_mask.yaml TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + PARAMS_FILE=${CMAKE_CURRENT_SOURCE_DIR}/keepout_params.yaml GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ASTAR=False