diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 836f7219df..e588ff0fa5 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -8,6 +8,7 @@ find_package(nav_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(std_msgs REQUIRED) +find_package(geographic_msgs) find_package(action_msgs REQUIRED) nav2_package() @@ -49,7 +50,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/Spin.action" "action/DummyBehavior.action" "action/FollowWaypoints.action" - DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs + "action/FollowGPSWaypoints.action" + DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs ) ament_export_dependencies(rosidl_default_runtime) diff --git a/nav2_msgs/action/FollowGPSWaypoints.action b/nav2_msgs/action/FollowGPSWaypoints.action new file mode 100644 index 0000000000..707e9a2b37 --- /dev/null +++ b/nav2_msgs/action/FollowGPSWaypoints.action @@ -0,0 +1,17 @@ +#goal definition +uint32 number_of_loops +uint32 goal_index 0 +geographic_msgs/GeoPose[] gps_poses +--- +#result definition + +# Error codes +# Note: The expected priority order of the errors should match the message order +uint16 NONE=0 +uint16 UNKNOWN=600 +uint16 TASK_EXECUTOR_FAILED=601 + +MissedWaypoint[] missed_waypoints +--- +#feedback +uint32 current_waypoint diff --git a/nav2_msgs/package.xml b/nav2_msgs/package.xml index bc3391f466..48e555532b 100644 --- a/nav2_msgs/package.xml +++ b/nav2_msgs/package.xml @@ -19,6 +19,7 @@ geometry_msgs action_msgs nav_msgs + geographic_msgs rosidl_interface_packages diff --git a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py index d5daa3cb09..c0c09d80c2 100644 --- a/nav2_simple_commander/nav2_simple_commander/robot_navigator.py +++ b/nav2_simple_commander/nav2_simple_commander/robot_navigator.py @@ -25,7 +25,8 @@ from lifecycle_msgs.srv import GetState from nav2_msgs.action import AssistedTeleop, BackUp, Spin from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose -from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose +from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \ + NavigateThroughPoses, NavigateToPose from nav2_msgs.action import SmoothPath from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes @@ -67,6 +68,8 @@ def __init__(self, node_name='basic_navigator'): 'navigate_through_poses') self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose') self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints') + self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints, + 'follow_gps_waypoints') self.follow_path_client = ActionClient(self, FollowPath, 'follow_path') self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose, 'compute_path_to_pose') @@ -182,6 +185,28 @@ def followWaypoints(self, poses): self.result_future = self.goal_handle.get_result_async() return True + def followGpsWaypoints(self, gps_poses): + """Send a `FollowGPSWaypoints` action request.""" + self.debug("Waiting for 'FollowWaypoints' action server") + while not self.follow_gps_waypoints_client.wait_for_server(timeout_sec=1.0): + self.info("'FollowWaypoints' action server not available, waiting...") + + goal_msg = FollowGPSWaypoints.Goal() + goal_msg.gps_poses = gps_poses + + self.info(f'Following {len(goal_msg.gps_poses)} gps goals....') + send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg, + self._feedbackCallback) + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + + if not self.goal_handle.accepted: + self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!') + return False + + self.result_future = self.goal_handle.get_result_async() + return True + def spin(self, spin_dist=1.57, time_allowance=10): self.debug("Waiting for 'Spin' action server") while not self.spin_client.wait_for_server(timeout_sec=1.0): @@ -310,7 +335,8 @@ def getResult(self): def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'): """Block until the full navigation system is up and running.""" - self._waitForNodeToActivate(localizer) + if localizer != "robot_localization": # non-lifecycle node + self._waitForNodeToActivate(localizer) if localizer == 'amcl': self._waitForInitialPose() self._waitForNodeToActivate(navigator) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 352f9f2277..2674b65e74 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -114,6 +114,7 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) + add_subdirectory(src/gps_navigation) add_subdirectory(src/behaviors/spin) add_subdirectory(src/behaviors/wait) add_subdirectory(src/behaviors/backup) diff --git a/nav2_system_tests/src/gps_navigation/CMakeLists.txt b/nav2_system_tests/src/gps_navigation/CMakeLists.txt new file mode 100644 index 0000000000..3342b6cd58 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/CMakeLists.txt @@ -0,0 +1,11 @@ +ament_add_test(test_gps_waypoint_follower + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR} + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml +) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py new file mode 100644 index 0000000000..852436615b --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py @@ -0,0 +1,63 @@ +# Copyright 2018 Open Source Robotics Foundation, Inc. +# 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. + +from launch import LaunchDescription +import launch_ros.actions +import os +import launch.actions + + +def generate_launch_description(): + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml") + os.environ["FILE_PATH"] = str(launch_dir) + return LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + "output_final_position", default_value="false" + ), + launch.actions.DeclareLaunchArgument( + "output_location", default_value="~/dual_ekf_navsat_example_debug.txt" + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_odom", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/local")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="ekf_node", + name="ekf_filter_node_map", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[("odometry/filtered", "odometry/global")], + ), + launch_ros.actions.Node( + package="robot_localization", + executable="navsat_transform_node", + name="navsat_transform", + output="screen", + parameters=[params_file, {"use_sim_time": True}], + remappings=[ + ("imu/data", "imu/data"), + ("gps/fix", "gps/fix"), + ("gps/filtered", "gps/filtered"), + ("odometry/gps", "odometry/gps"), + ("odometry/filtered", "odometry/global"), + ], + ), + ] + ) diff --git a/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml new file mode 100644 index 0000000000..6783a7b1db --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml @@ -0,0 +1,127 @@ +# For parameter descriptions, please refer to the template parameter files for each node. + +ekf_filter_node_odom: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] + +ekf_filter_node_map: + ros__parameters: + frequency: 30.0 + two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d + print_diagnostics: true + debug: false + publish_tf: true + + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + odom0: odom + odom0_config: [false, false, false, + false, false, false, + true, true, true, + false, false, true, + false, false, false] + odom0_queue_size: 10 + odom0_differential: false + odom0_relative: false + + odom1: odometry/gps + odom1_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom1_queue_size: 10 + odom1_differential: false + odom1_relative: false + + imu0: imu/data + imu0_config: [false, false, false, + false, false, true, + false, false, false, + false, false, false, + false, false, false] + imu0_differential: false # If using a real robot you might want to set this to true, since usually absolute measurements from real imu's are not very accurate + imu0_relative: false + imu0_queue_size: 10 + imu0_remove_gravitational_acceleration: true + + use_control: false + + process_noise_covariance: [1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1e-3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.3] + +navsat_transform: + ros__parameters: + frequency: 30.0 + delay: 3.0 + magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998 + yaw_offset: 0.0 + zero_altitude: true + broadcast_utm_transform: true + publish_filtered_gps: true + use_odometry_yaw: true + wait_for_datum: false diff --git a/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml new file mode 100644 index 0000000000..2539cef7b5 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/nav2_no_map_params.yaml @@ -0,0 +1,307 @@ +bt_navigator: + ros__parameters: + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + navigators: ["navigate_to_pose", "navigate_through_poses"] + navigate_to_pose: + plugin: "nav2_bt_navigator/NavigateToPoseNavigator" + navigate_through_poses: + plugin: "nav2_bt_navigator/NavigateThroughPosesNavigator" + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_are_error_codes_active_condition_bt_node + - nav2_would_a_controller_recovery_help_condition_bt_node + - nav2_would_a_planner_recovery_help_condition_bt_node + - nav2_would_a_smoother_recovery_help_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + error_code_names: + - compute_path_error_code + - follow_path_error_code + +controller_server: + ros__parameters: + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugins: ["progress_checker"] + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + robot_radius: 0.22 + resolution: 0.1 + # When using GPS navigation you will potentially traverse huge environments which are not practical to + # fit on a big static costmap. Thus it is recommended to use a rolling global costmap large enough to + # contain each pair of successive waypoints. See: https://github.com/ros-planning/navigation2/issues/2174 + rolling_window: True + width: 50 + height: 50 + track_unknown_space: true + # no static map + plugins: ["obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + # outdoors there will probably be more inf points + inf_is_valid: true + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +# The yaml_filename does not need to be specified since it going to be set by defaults in launch. +# If you'd rather set it in the yaml, remove the default "map" value in the tb3_simulation_launch.py +# file & provide full path to map below. If CLI map configuration or launch default is provided, that will be used. +# map_server: +# ros__parameters: +# yaml_filename: "" + +map_saver: + ros__parameters: + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + local_costmap_topic: local_costmap/costmap_raw + global_costmap_topic: global_costmap/costmap_raw + local_footprint_topic: local_costmap/published_footprint + global_footprint_topic: global_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + local_frame: odom + global_frame: map + robot_base_frame: base_link + transform_tolerance: 0.1 + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +waypoint_follower: + ros__parameters: + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/nav2_system_tests/src/gps_navigation/test_case_py.launch.py b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py new file mode 100755 index 0000000000..cd90fdb620 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/test_case_py.launch.py @@ -0,0 +1,132 @@ +#! /usr/bin/env python3 +# Copyright (c) 2019 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(): + world = os.getenv("TEST_WORLD") + + launch_dir = os.path.dirname(os.path.realpath(__file__)) + params_file = os.path.join(launch_dir, "nav2_no_map_params.yaml") + bringup_dir = get_package_share_directory("nav2_bringup") + + configured_params = RewrittenYaml( + source_file=params_file, root_key="", param_rewrites="", convert_types=True + ) + + return LaunchDescription( + [ + SetEnvironmentVariable("RCUTILS_LOGGING_BUFFERED_STREAM", "1"), + SetEnvironmentVariable("RCUTILS_LOGGING_USE_STDOUT", "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"], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=[ + "-0.32", + "0", + "0.068", + "0", + "0", + "0", + "base_link", + "imu_link", + ], + ), + Node( + package="tf2_ros", + executable="static_transform_publisher", + output="screen", + arguments=["0", "0", "0", "0", "0", "0", "base_link", "gps_link"], + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(bringup_dir, "launch", "navigation_launch.py") + ), + launch_arguments={ + "use_sim_time": "True", + "params_file": configured_params, + "autostart": "True", + }.items(), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, "dual_ekf_navsat.launch.py") + ), + ), + ] + ) + + +def main(argv=sys.argv[1:]): + ld = generate_launch_description() + + test1_action = ExecuteProcess( + cmd=[os.path.join(os.getenv("TEST_DIR"), "tester.py")], + 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/gps_navigation/tester.py b/nav2_system_tests/src/gps_navigation/tester.py new file mode 100755 index 0000000000..aab8833668 --- /dev/null +++ b/nav2_system_tests/src/gps_navigation/tester.py @@ -0,0 +1,222 @@ +#! /usr/bin/env python3 +# Copyright 2019 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 sys +import time + +from action_msgs.msg import GoalStatus +from geographic_msgs.msg import GeoPose +from nav2_msgs.action import ComputePathToPose, FollowGPSWaypoints +from nav2_msgs.srv import ManageLifecycleNodes +from rcl_interfaces.srv import SetParameters +from rclpy.parameter import Parameter + +import rclpy +from rclpy.action import ActionClient +from rclpy.node import Node + + +class GpsWaypointFollowerTest(Node): + def __init__(self): + super().__init__(node_name="nav2_gps_waypoint_tester", namespace="") + self.waypoints = None + self.action_client = ActionClient( + self, FollowGPSWaypoints, "follow_gps_waypoints" + ) + self.goal_handle = None + self.action_result = None + + self.param_cli = self.create_client(SetParameters, + '/waypoint_follower/set_parameters') + + def setWaypoints(self, waypoints): + self.waypoints = [] + for wp in waypoints: + msg = GeoPose() + msg.position.latitude = wp[0] + msg.position.longitude = wp[1] + msg.orientation.w = 1.0 + self.waypoints.append(msg) + + def run(self, block, cancel): + # if not self.waypoints: + # rclpy.error_msg("Did not set valid waypoints before running test!") + # return False + + while not self.action_client.wait_for_server(timeout_sec=1.0): + self.info_msg( + "'follow_gps_waypoints' action server not available, waiting...") + + action_request = FollowGPSWaypoints.Goal() + action_request.gps_poses = self.waypoints + + self.info_msg("Sending goal request...") + send_goal_future = self.action_client.send_goal_async(action_request) + try: + rclpy.spin_until_future_complete(self, send_goal_future) + self.goal_handle = send_goal_future.result() + except Exception as e: # noqa: B902 + self.error_msg(f"Service call failed {e!r}") + + if not self.goal_handle.accepted: + self.error_msg("Goal rejected") + return False + + self.info_msg("Goal accepted") + if not block: + return True + + get_result_future = self.goal_handle.get_result_async() + if cancel: + time.sleep(2) + self.cancel_goal() + + self.info_msg("Waiting for 'follow_gps_waypoints' action to complete") + try: + rclpy.spin_until_future_complete(self, get_result_future) + status = get_result_future.result().status + result = get_result_future.result().result + self.action_result = result + except Exception as e: # noqa: B902 + self.error_msg(f"Service call failed {e!r}") + + if status != GoalStatus.STATUS_SUCCEEDED: + self.info_msg(f"Goal failed with status code: {status}") + return False + if len(result.missed_waypoints) > 0: + self.info_msg( + "Goal failed to process all waypoints," + " missed {0} wps.".format(len(result.missed_waypoints)) + ) + return False + + self.info_msg("Goal succeeded!") + return True + + def setStopFailureParam(self, value): + req = SetParameters.Request() + req.parameters = [Parameter('stop_on_failure', + Parameter.Type.BOOL, value).to_parameter_msg()] + future = self.param_cli.call_async(req) + rclpy.spin_until_future_complete(self, future) + + def shutdown(self): + self.info_msg("Shutting down") + + self.action_client.destroy() + self.info_msg("Destroyed follow_gps_waypoints action client") + + 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...") + + req = ManageLifecycleNodes.Request() + req.command = ManageLifecycleNodes.Request().SHUTDOWN + future = mgr_client.call_async(req) + try: + rclpy.spin_until_future_complete(self, future) + future.result() + except Exception as e: # noqa: B902 + self.error_msg(f"{transition_service} service call failed {e!r}") + + self.info_msg(f"{transition_service} finished") + + 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(msg) + + def warn_msg(self, msg: str): + self.get_logger().warn(msg) + + def error_msg(self, msg: str): + self.get_logger().error(msg) + + +def main(argv=sys.argv[1:]): + rclpy.init() + + # wait a few seconds to make sure entire stacks are up + time.sleep(10) + + wps = [[55.944831, -3.186998], [55.944818, -3.187075], [55.944782, -3.187060]] + + test = GpsWaypointFollowerTest() + test.setWaypoints(wps) + + # wait for poseCallback + + result = test.run(True, False) + assert result + + # preempt with new point + test.setWaypoints([wps[0]]) + result = test.run(False, False) + time.sleep(2) + test.setWaypoints([wps[1]]) + result = test.run(False, False) + + # cancel + time.sleep(2) + test.cancel_goal() + + # set waypoint outside of map + time.sleep(2) + test.setWaypoints([[35.0, -118.0]]) + result = test.run(True, False) + assert not result + result = not result + assert test.action_result.missed_waypoints[0].error_code == \ + ComputePathToPose.Result().GOAL_OUTSIDE_MAP + + # stop on failure test with bogous waypoint + test.setStopFailureParam(True) + bwps = [[55.944831, -3.186998], [35.0, -118.0], [55.944782, -3.187060]] + test.setWaypoints(bwps) + result = test.run(True, False) + assert not result + result = not result + mwps = test.action_result.missed_waypoints + result = (len(mwps) == 1) & (mwps[0] == 1) + test.setStopFailureParam(False) + + # Zero goal test + test.setWaypoints([]) + result = test.run(True, False) + + # Cancel test + test.setWaypoints(wps) + result = test.run(True, True) + assert not result + result = not result + + test.shutdown() + test.info_msg("Done Shutting Down.") + + if not result: + test.info_msg("Exiting failed") + exit(1) + else: + test.info_msg("Exiting passed") + exit(0) + + +if __name__ == "__main__": + main() diff --git a/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world new file mode 100644 index 0000000000..94b72499a2 --- /dev/null +++ b/nav2_system_tests/worlds/turtlebot3_ros2_demo_gps.world @@ -0,0 +1,555 @@ + + + + + + + EARTH_WGS84 + ENU + 55.944831 + -3.186998 + 0.0 + 180.0 + + + + model://ground_plane + + + + model://sun + + + + false + + + + + 0.319654 -0.235002 9.29441 0 1.5138 0.009599 + orbit + perspective + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + -2.0 -0.5 0.01 0.0 0.0 0.0 + + + + + + + -0.064 0 0.048 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 1.0 + + + + -0.064 0 0.048 0 0 0 + + + 0.265 0.265 0.089 + + + + + + -0.064 0 0 0 0 0 + + + model://turtlebot3_waffle/meshes/waffle_base.dae + 0.001 0.001 0.001 + + + + + + + + true + 200 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + 0.0 + 2e-4 + + + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + 0.0 + 1.7e-2 + + + + + + false + + + ~/out:=/imu/data + + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + true + 1 + 0 0 0 0 0 0 + + + + + 0.0 + 0.1 + + + + + 0.0 + 0.0 + + + + + + + ~/out:=/gps/fix + + + + + + + + -0.052 0 0.111 0 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.125 + + + + -0.064 0 0.121 0 0 0 + + + 0.0508 + 0.055 + + + + + + -0.032 0 0.171 0 0 0 + + + model://turtlebot3_waffle/meshes/lds.dae + 0.001 0.001 0.001 + + + + + + true + false + -0.064 0 0.121 0 0 0 + 5 + + + + 360 + 1.000000 + 0.000000 + 6.280000 + + + + 0.120000 + 3.5 + 0.015000 + + + gaussian + 0.0 + 0.01 + + + + + + ~/out:=scan + + sensor_msgs/LaserScan + + + + + + + + 0.0 0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/left_tire.dae + 0.001 0.001 0.001 + + + + + + + + + 0.0 -0.144 0.023 -1.57 0 0 + + 0.001 + 0.000 + 0.000 + 0.001 + 0.000 + 0.001 + + 0.1 + + + + 0.0 -0.144 0.023 -1.57 0 0 + + + 0.033 + 0.018 + + + + + + + 100000.0 + 100000.0 + 0 0 0 + 0.0 + 0.0 + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + 0.0 -0.144 0.023 0 0 0 + + + model://turtlebot3_waffle/meshes/right_tire.dae + 0.001 0.001 0.001 + + + + + + + -0.177 -0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + -0.177 0.064 -0.004 0 0 0 + + 0.001 + + 0.00001 + 0.000 + 0.000 + 0.00001 + 0.000 + 0.00001 + + + + + + 0.005000 + + + + + + 0 + 0.2 + 1e+5 + 1 + 0.01 + 0.001 + + + + + + + + base_footprint + base_link + 0.0 0.0 0.010 0 0 0 + + + + base_link + wheel_left_link + 0.0 0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + wheel_right_link + 0.0 -0.144 0.023 -1.57 0 0 + + 0 0 1 + + + + + base_link + caster_back_right_link + + + + base_link + caster_back_left_link + + + + base_link + imu_link + -0.032 0 0.068 0 0 0 + + 0 0 1 + + + + + base_link + gps_link + -0.05 0 0.05 0 0 0 + + 0 0 1 + + + + + base_link + base_scan + -0.064 0 0.121 0 0 0 + + 0 0 1 + + + + + + + + /tf:=tf + + + + wheel_left_joint + wheel_right_joint + + + 0.287 + 0.066 + + + 20 + 1.0 + + + true + false + false + + odom + base_link + + + + + + + ~/out:=joint_states + + 250 + wheel_left_joint + wheel_right_joint + + + + + + + diff --git a/nav2_util/include/nav2_util/service_client.hpp b/nav2_util/include/nav2_util/service_client.hpp index be7340e8fb..6f25965d1e 100644 --- a/nav2_util/include/nav2_util/service_client.hpp +++ b/nav2_util/include/nav2_util/service_client.hpp @@ -26,7 +26,7 @@ namespace nav2_util * @class nav2_util::ServiceClient * @brief A simple wrapper on ROS2 services for invoke() and block-style calling */ -template +template class ServiceClient { public: @@ -37,14 +37,14 @@ class ServiceClient */ explicit ServiceClient( const std::string & service_name, - const rclcpp::Node::SharedPtr & provided_node) + const NodeT & provided_node) : service_name_(service_name), node_(provided_node) { callback_group_ = node_->create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); callback_group_executor_.add_callback_group(callback_group_, node_->get_node_base_interface()); - client_ = node_->create_client( + client_ = node_->template create_client( service_name, rclcpp::SystemDefaultsQoS(), callback_group_); @@ -147,7 +147,7 @@ class ServiceClient protected: std::string service_name_; - rclcpp::Node::SharedPtr node_; + NodeT node_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; typename rclcpp::Client::SharedPtr client_; diff --git a/nav2_waypoint_follower/CMakeLists.txt b/nav2_waypoint_follower/CMakeLists.txt index a3b46942bc..1d5405878b 100644 --- a/nav2_waypoint_follower/CMakeLists.txt +++ b/nav2_waypoint_follower/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(nav2_util REQUIRED) find_package(tf2_ros REQUIRED) find_package(nav2_core REQUIRED) find_package(pluginlib REQUIRED) +find_package(robot_localization REQUIRED) +find_package(geographic_msgs REQUIRED) nav2_package() @@ -57,6 +59,7 @@ set(dependencies image_transport cv_bridge OpenCV + robot_localization ) ament_target_dependencies(${executable_name} diff --git a/nav2_waypoint_follower/README.md b/nav2_waypoint_follower/README.md index cd0a2366b9..5d6a049857 100644 --- a/nav2_waypoint_follower/README.md +++ b/nav2_waypoint_follower/README.md @@ -27,3 +27,22 @@ In the first, the ``nav2_waypoint_follower`` is weakly sufficient to create a pr In the second, the ``nav2_waypoint_follower`` is a nice sample application / proof of concept, but you really need your waypoint following / autonomy system on the robot to carry more weight in making a robust solution. In this case, you should use the ``nav2_behavior_tree`` package to create a custom application-level behavior tree using navigation to complete the task. This can include subtrees like checking for the charge status mid-task for returning to dock or handling more than 1 unit of work in a more complex task. Soon, there will be a ``nav2_bt_waypoint_follower`` (name subject to adjustment) that will allow you to create this application more easily. In this school of thought, the waypoint following application is more closely tied to the system autonomy, or in many cases, is the system autonomy. Neither is better than the other, it highly depends on the tasks your robot(s) are completing, in what type of environment, and with what cloud resources available. Often this distinction is very clear for a given business case. + +## Nav2 GPS Waypoint Follower + +`nav2_waypoint_follower` provides an action server named `FollowGPSWaypoints` which accepts GPS waypoint following requests by using tools provided by `robot_localization` and `nav2_waypoint_follower` itself. + +`robot_localization`'s `navsat_transform_node` provides a service `fromLL`, which is used to convert pure GPS coordinates(longitude, latitude, alitude) +to cartesian coordinates in map frame(x,y), then the existent action named `FollowWaypoints` from `nav2_waypoint_follower` is used to get robot go through each converted waypoints. +The action msg definition for GPS waypoint following can be found [here](../nav2_msgs/action/FollowGPSWaypoints.action). + +In a common use case, an client node can read a set of GPS waypoints from a YAML file an create a client to action server named as `FollowGPSWaypoints`. +For instance, + +```cpp +using ClientT = nav2_msgs::action::FollowGPSWaypoints; +rclcpp_action::Client::SharedPtr gps_waypoint_follower_action_client_; +gps_waypoint_follower_action_client_ = rclcpp_action::create_client(this, "follow_gps_waypoints"); +``` + +All other functionalities provided by `nav2_waypoint_follower` such as WaypointTaskExecutors are usable and can be configured in WaypointTaskExecutor. \ No newline at end of file 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 e5c59aa20b..8dd0d70cd7 100644 --- a/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp +++ b/nav2_waypoint_follower/include/nav2_waypoint_follower/waypoint_follower.hpp @@ -20,18 +20,26 @@ #include #include +#include "rclcpp_action/rclcpp_action.hpp" +#include "pluginlib/class_loader.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "geographic_msgs/msg/geo_pose.hpp" #include "nav2_util/lifecycle_node.hpp" #include "nav2_msgs/action/navigate_to_pose.hpp" #include "nav2_msgs/action/follow_waypoints.hpp" #include "nav2_msgs/msg/missed_waypoint.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_util/simple_action_server.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - #include "nav2_util/node_utils.hpp" +#include "nav2_util/string_utils.hpp" +#include "nav2_msgs/action/follow_gps_waypoints.hpp" +#include "nav2_util/service_client.hpp" #include "nav2_core/waypoint_task_executor.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" + +#include "robot_localization/srv/from_ll.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/transform_listener.h" namespace nav2_waypoint_follower { @@ -63,6 +71,10 @@ class WaypointFollower : public nav2_util::LifecycleNode using ActionServer = nav2_util::SimpleActionServer; using ActionClient = rclcpp_action::Client; + // Shorten the types for GPS waypoint following + using ActionTGPS = nav2_msgs::action::FollowGPSWaypoints; + using ActionServerGPS = nav2_util::SimpleActionServer; + /** * @brief A constructor for nav2_waypoint_follower::WaypointFollower class * @param options Additional options to control creation of the node. @@ -107,15 +119,40 @@ class WaypointFollower : public nav2_util::LifecycleNode */ nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override; + /** + * @brief Templated function to perform internal logic behind waypoint following, + * Both GPS and non GPS waypoint following callbacks makes use of this function when a client asked to do so. + * Callbacks fills in appropriate types for the tempelated types, see followWaypointCallback funtions for details. + * + * @tparam T action_server + * @tparam V feedback + * @tparam Z result + * @param action_server + * @param poses + * @param feedback + * @param result + */ + template + void followWaypointsHandler(const T & action_server, const V & feedback, const Z & result); + /** * @brief Action server callbacks */ - void followWaypoints(); + void followWaypointsCallback(); /** - * @brief Action client result callback - * @param result Result of action server updated asynchronously + * @brief send robot through each of GPS + * point , which are converted to map frame first then using a client to + * `FollowWaypoints` action. + * + * @param waypoints, acquired from action client */ + void followGPSWaypointsCallback(); + + /** + * @brief Action client result callback + * @param result Result of action server updated asynchronously + */ void resultCallback(const rclcpp_action::ClientGoalHandle::WrappedResult & result); /** @@ -124,6 +161,32 @@ class WaypointFollower : public nav2_util::LifecycleNode */ void goalResponseCallback(const rclcpp_action::ClientGoalHandle::SharedPtr & goal); + /** + * @brief given some gps_poses, converts them to map frame using robot_localization's service `fromLL`. + * Constructs a vector of stamped poses in map frame and returns them. + * + * @param gps_poses, from the action server + * @return std::vector + */ + std::vector convertGPSPosesToMapPoses( + const std::vector & gps_poses); + + + /** + * @brief get the latest poses on the action server goal. If they are GPS poses, + * convert them to the global cartesian frame using /fromLL robot localization + * server + * + * @param action_server, to which the goal was sent + * @return std::vector + */ + template + std::vector getLatestGoalPoses(const T & action_server); + + // Common vars used for both GPS and cartesian point following + std::vector failed_ids_; + std::string global_frame_id_{"map"}; + /** * @brief Callback executed when a parameter change is detected * @param event ParameterEvent message @@ -135,12 +198,17 @@ class WaypointFollower : public nav2_util::LifecycleNode rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; // Our action server - std::unique_ptr action_server_; + std::unique_ptr xyz_action_server_; ActionClient::SharedPtr nav_to_pose_client_; rclcpp::CallbackGroup::SharedPtr callback_group_; rclcpp::executors::SingleThreadedExecutor callback_group_executor_; std::shared_future::SharedPtr> future_goal_handle_; + // Our action server for GPS waypoint following + std::unique_ptr gps_action_server_; + std::unique_ptr>> from_ll_to_map_client_; + bool stop_on_failure_; int loop_rate_; GoalStatus current_goal_status_; diff --git a/nav2_waypoint_follower/package.xml b/nav2_waypoint_follower/package.xml index 42f23858d1..8dfbe41f44 100644 --- a/nav2_waypoint_follower/package.xml +++ b/nav2_waypoint_follower/package.xml @@ -21,7 +21,9 @@ nav2_util nav2_core tf2_ros - + robot_localization + geographic_msgs + ament_lint_common ament_lint_auto ament_cmake_gtest diff --git a/nav2_waypoint_follower/src/waypoint_follower.cpp b/nav2_waypoint_follower/src/waypoint_follower.cpp index 42342638f0..f462a771a3 100644 --- a/nav2_waypoint_follower/src/waypoint_follower.cpp +++ b/nav2_waypoint_follower/src/waypoint_follower.cpp @@ -39,6 +39,8 @@ WaypointFollower::WaypointFollower(const rclcpp::NodeOptions & options) declare_parameter("action_server_result_timeout", 900.0); + declare_parameter("global_frame_id", "map"); + nav2_util::declare_parameter_if_not_declared( this, std::string("waypoint_task_executor_plugin"), rclcpp::ParameterValue(std::string("wait_at_waypoint"))); @@ -61,6 +63,8 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) stop_on_failure_ = get_parameter("stop_on_failure").as_bool(); loop_rate_ = get_parameter("loop_rate").as_int(); waypoint_task_executor_id_ = get_parameter("waypoint_task_executor_plugin").as_string(); + global_frame_id_ = get_parameter("global_frame_id").as_string(); + global_frame_id_ = nav2_util::strip_leading_slash(global_frame_id_); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, @@ -79,13 +83,30 @@ WaypointFollower::on_configure(const rclcpp_lifecycle::State & /*state*/) rcl_action_server_options_t server_options = rcl_action_server_get_default_options(); server_options.result_timeout.nanoseconds = RCL_S_TO_NS(action_server_result_timeout); - action_server_ = std::make_unique( + xyz_action_server_ = std::make_unique( get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), get_node_waitables_interface(), "follow_waypoints", std::bind( - &WaypointFollower::followWaypoints, + &WaypointFollower::followWaypointsCallback, + this), nullptr, std::chrono::milliseconds( + 500), false, server_options); + + from_ll_to_map_client_ = std::make_unique< + nav2_util::ServiceClient>>( + "/fromLL", + node); + + gps_action_server_ = std::make_unique( + get_node_base_interface(), + get_node_clock_interface(), + get_node_logging_interface(), + get_node_waitables_interface(), + "follow_gps_waypoints", + std::bind( + &WaypointFollower::followGPSWaypointsCallback, this), nullptr, std::chrono::milliseconds( 500), false, server_options); @@ -113,7 +134,8 @@ WaypointFollower::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - action_server_->activate(); + xyz_action_server_->activate(); + gps_action_server_->activate(); auto node = shared_from_this(); // Add callback for dynamic parameters @@ -131,9 +153,9 @@ WaypointFollower::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - action_server_->deactivate(); + xyz_action_server_->deactivate(); + gps_action_server_->deactivate(); dyn_params_handler_.reset(); - // destroy bond connection destroyBond(); @@ -145,8 +167,10 @@ WaypointFollower::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - action_server_.reset(); + xyz_action_server_.reset(); nav_to_pose_client_.reset(); + gps_action_server_.reset(); + from_ll_to_map_client_.reset(); return nav2_util::CallbackReturn::SUCCESS; } @@ -158,29 +182,55 @@ WaypointFollower::on_shutdown(const rclcpp_lifecycle::State & /*state*/) return nav2_util::CallbackReturn::SUCCESS; } -void -WaypointFollower::followWaypoints() +template +std::vector WaypointFollower::getLatestGoalPoses( + const T & action_server) { - auto goal = action_server_->get_current_goal(); - auto feedback = std::make_shared(); - auto result = std::make_shared(); + std::vector poses; + + // compile time static check to decide which block of code to be built + if constexpr (std::is_same>::value) { + // If normal waypoint following callback was called, we build here + poses = action_server->get_current_goal()->poses; + } else { + // If GPS waypoint following callback was called, we build here + poses = convertGPSPosesToMapPoses( + action_server->get_current_goal()->gps_poses); + } + return poses; +} + +template +void WaypointFollower::followWaypointsHandler( + const T & action_server, + const V & feedback, + const Z & result) +{ + auto goal = action_server->get_current_goal(); // handling loops unsigned int current_loop_no = 0; auto no_of_loops = goal->number_of_loops; - // Check if request is valid - if (!action_server_ || !action_server_->is_server_active()) { + std::vector poses; + poses = getLatestGoalPoses(action_server); + + if (!action_server || !action_server->is_server_active()) { RCLCPP_DEBUG(get_logger(), "Action server inactive. Stopping."); return; } RCLCPP_INFO( get_logger(), "Received follow waypoint request with %i waypoints.", - static_cast(goal->poses.size())); + static_cast(poses.size())); - if (goal->poses.size() == 0) { - action_server_->succeeded_current(result); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of waypoints passed to waypoint following " + "action potentially due to conversation failure or empty request." + ); + action_server->terminate_current(result); return; } @@ -192,20 +242,29 @@ WaypointFollower::followWaypoints() while (rclcpp::ok()) { // Check if asked to stop processing action - if (action_server_->is_cancel_requested()) { + if (action_server->is_cancel_requested()) { auto cancel_future = nav_to_pose_client_->async_cancel_all_goals(); callback_group_executor_.spin_until_future_complete(cancel_future); // for result callback processing callback_group_executor_.spin_some(); - action_server_->terminate_all(); + action_server->terminate_all(); return; } // Check if asked to process another action - if (action_server_->is_preempt_requested()) { + if (action_server->is_preempt_requested()) { RCLCPP_INFO(get_logger(), "Preempting the goal pose."); - goal = action_server_->accept_pending_goal(); - goal_index = goal->goal_index; + goal = action_server->accept_pending_goal(); + poses = getLatestGoalPoses(action_server); + if (poses.empty()) { + RCLCPP_ERROR( + get_logger(), + "Empty vector of Waypoints passed to waypoint following logic. " + "Nothing to execute, returning with failure!"); + action_server->terminate_current(result); + return; + } + goal_index = 0; new_goal = true; } @@ -213,20 +272,24 @@ WaypointFollower::followWaypoints() if (new_goal) { new_goal = false; ClientT::Goal client_goal; - client_goal.pose = goal->poses[goal_index]; + client_goal.pose = poses[goal_index]; + client_goal.pose.header.stamp = this->now(); auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - std::bind(&WaypointFollower::resultCallback, this, std::placeholders::_1); - send_goal_options.goal_response_callback = - std::bind(&WaypointFollower::goalResponseCallback, this, std::placeholders::_1); + send_goal_options.result_callback = std::bind( + &WaypointFollower::resultCallback, this, + std::placeholders::_1); + send_goal_options.goal_response_callback = std::bind( + &WaypointFollower::goalResponseCallback, + this, std::placeholders::_1); + future_goal_handle_ = nav_to_pose_client_->async_send_goal(client_goal, send_goal_options); current_goal_status_.status = ActionStatus::PROCESSING; } feedback->current_waypoint = goal_index; - action_server_->publish_feedback(feedback); + action_server->publish_feedback(feedback); if ( current_goal_status_.status == ActionStatus::FAILED || @@ -234,7 +297,7 @@ WaypointFollower::followWaypoints() { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = current_goal_status_.error_code; result->missed_waypoints.push_back(missedWaypoint); @@ -243,7 +306,7 @@ WaypointFollower::followWaypoints() get_logger(), "Failed to process waypoint %i in waypoint " "list and stop on failure is enabled." " Terminating action.", goal_index); - action_server_->terminate_current(result); + action_server->terminate_current(result); current_goal_status_.error_code = 0; return; } else { @@ -256,7 +319,7 @@ WaypointFollower::followWaypoints() get_logger(), "Succeeded processing waypoint %i, processing waypoint task execution", goal_index); bool is_task_executed = waypoint_task_executor_->processAtWaypoint( - goal->poses[goal_index], goal_index); + poses[goal_index], goal_index); RCLCPP_INFO( get_logger(), "Task execution at waypoint %i %s", goal_index, is_task_executed ? "succeeded" : "failed!"); @@ -264,7 +327,7 @@ WaypointFollower::followWaypoints() if (!is_task_executed) { nav2_msgs::msg::MissedWaypoint missedWaypoint; missedWaypoint.index = goal_index; - missedWaypoint.goal = goal->poses[goal_index]; + missedWaypoint.goal = poses[goal_index]; missedWaypoint.error_code = nav2_msgs::action::FollowWaypoints::Result::TASK_EXECUTOR_FAILED; result->missed_waypoints.push_back(missedWaypoint); @@ -276,7 +339,7 @@ WaypointFollower::followWaypoints() " stop on failure is enabled." " Terminating action.", goal_index); - action_server_->terminate_current(result); + action_server->terminate_current(result); current_goal_status_.error_code = 0; return; } else { @@ -290,12 +353,12 @@ WaypointFollower::followWaypoints() // Update server state goal_index++; new_goal = true; - if (goal_index >= goal->poses.size()) { + if (goal_index >= poses.size()) { if (current_loop_no == no_of_loops) { RCLCPP_INFO( get_logger(), "Completed all %zu waypoints requested.", - goal->poses.size()); - action_server_->succeeded_current(result); + poses.size()); + action_server->succeeded_current(result); current_goal_status_.error_code = 0; return; } @@ -312,6 +375,30 @@ WaypointFollower::followWaypoints() } } +void WaypointFollower::followWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsHandler, + ActionT::Feedback::SharedPtr, + ActionT::Result::SharedPtr>( + xyz_action_server_, + feedback, result); +} + +void WaypointFollower::followGPSWaypointsCallback() +{ + auto feedback = std::make_shared(); + auto result = std::make_shared(); + + followWaypointsHandler, + ActionTGPS::Feedback::SharedPtr, + ActionTGPS::Result::SharedPtr>( + gps_action_server_, + feedback, result); +} + void WaypointFollower::resultCallback( const rclcpp_action::ClientGoalHandle::WrappedResult & result) @@ -379,6 +466,58 @@ WaypointFollower::dynamicParametersCallback(std::vector param return result; } +std::vector +WaypointFollower::convertGPSPosesToMapPoses( + const std::vector & gps_poses) +{ + RCLCPP_INFO( + this->get_logger(), "Converting GPS waypoints to %s Frame..", + global_frame_id_.c_str()); + + std::vector poses_in_map_frame_vector; + int waypoint_index = 0; + for (auto && curr_geopose : gps_poses) { + auto request = std::make_shared(); + auto response = std::make_shared(); + request->ll_point.latitude = curr_geopose.position.latitude; + request->ll_point.longitude = curr_geopose.position.longitude; + request->ll_point.altitude = curr_geopose.position.altitude; + + from_ll_to_map_client_->wait_for_service((std::chrono::seconds(1))); + if (!from_ll_to_map_client_->invoke(request, response)) { + RCLCPP_ERROR( + this->get_logger(), + "fromLL service of robot_localization could not convert %i th GPS waypoint to" + "%s frame, going to skip this point!" + "Make sure you have run navsat_transform_node of robot_localization", + waypoint_index, global_frame_id_.c_str()); + if (stop_on_failure_) { + RCLCPP_ERROR( + this->get_logger(), + "Conversion of %i th GPS waypoint to" + "%s frame failed and stop_on_failure is set to true" + "Not going to execute any of waypoints, exiting with failure!", + waypoint_index, global_frame_id_.c_str()); + return std::vector(); + } + continue; + } else { + geometry_msgs::msg::PoseStamped curr_pose_map_frame; + curr_pose_map_frame.header.frame_id = global_frame_id_; + curr_pose_map_frame.header.stamp = this->now(); + curr_pose_map_frame.pose.position = response->map_point; + curr_pose_map_frame.pose.orientation = curr_geopose.orientation; + poses_in_map_frame_vector.push_back(curr_pose_map_frame); + } + waypoint_index++; + } + RCLCPP_INFO( + this->get_logger(), + "Converted all %i GPS waypoint to %s frame", + static_cast(poses_in_map_frame_vector.size()), global_frame_id_.c_str()); + return poses_in_map_frame_vector; +} + } // namespace nav2_waypoint_follower #include "rclcpp_components/register_node_macro.hpp" diff --git a/tools/underlay.repos b/tools/underlay.repos index 947aa5c66a..2e092f2517 100644 --- a/tools/underlay.repos +++ b/tools/underlay.repos @@ -19,10 +19,22 @@ repositories: # type: git # url: https://github.com/ros/bond_core.git # version: ros2 + # ros/diagnostics: + # type: git + # url: https://github.com/ros/diagnostics.git + # version: ros2-devel + # ros/geographic_info: + # type: git + # url: https://github.com/ros-geographic-info/geographic_info.git + # version: ros2 # ompl/ompl: # type: git # url: https://github.com/ompl/ompl.git # version: main + # robot_localization/robot_localization: + # type: git + # url: https://github.com/cra-ros-pkg/robot_localization.git + # version: ros2 # ros-simulation/gazebo_ros_pkgs: # type: git # url: https://github.com/ros-simulation/gazebo_ros_pkgs.git