Skip to content

Commit

Permalink
Add nav2_gps_waypoint_follower (#2814)
Browse files Browse the repository at this point in the history 
* Add nav2_gps_waypoint_follower

* use correct client node while calling it to spin

* changed after 1'st review

* apply requested changes

* nav2_util::ServiceClient instead of CallbackGroup

* another  iteration to adress issues

* update poses with function in the follower logic

* add deps of robot_localization: diagnostics

* fix typo in underlay.repo

* add deps of robot_localization: geographic_info

* minor clean-ups

* bond_core version has been updated

* rotation should also be considered in GPS WPFing

* use better namings related to gps wpf orientation

* handle cpplint errors

* tf_listener needs to be initialized

* apply requested changes

* apply requested changes 3.0/3.0

* fix misplaced ";"

* use run time param for gps transform timeout

* change timeout var name

* make use of stop_on_failure for GPS too

* passing emptywaypont vectors are seen as failure

* update warning for empty requests

* consider utm -> map yaw offset

* fix missed RCLCPP info

* reorrect action;s name

* waypoint stamps need to be updated

* Fix segmentation fault on waypoint follower

* Parametric frames and matrix multiplications

* Replace oriented navsatfix for geographic_msgs/geopose

* Remove deprecated oriented  navsatfix message

* Update branch name on robot_localization dependency

* Fix parametric frames logic

* Rename functions and adress comments

* fix style in underlay.repos

* remove duplicate word in underlay.repos

* update dependency version of ompl

* Template ServiceClient class to accept lifecycle node

* Remove link to stackoverflow answer

* Remove yaw offset compensation

* Fix API change

* Fix styling

* Minor docs fixes

* Fix style divergences

* Style fixes

* Style fixes v2

* Style fixes v3

* Remove unused variables and timestam overrides

* restore goal timestamp override

* WIP: Add follow gps waypoints test

* Style fixes and gazebo world inertia fix

* Reduce velocity smoother timeout

* empty commit to rerun tests

* Increment circle ci cache idx

* Remove extra space in cmakelists.txt

* Fix wrong usage of the global action server

* update follow gps waypoints action definition

* Fix action definition and looping

* update params for the unit testing

* WIP: update tests

* fix tests

* fixes to nav2 simple commander

* add robot_localization localizer

* Bring back from LL client

* Update nav2_simple_commander/nav2_simple_commander/robot_navigator.py

Co-authored-by: Steve Macenski <[email protected]>

* missing argument in test function

* small test error

* style fixes nav2 simple commander

* rename cartesian action server

---------

Co-authored-by: jediofgever <[email protected]>
Co-authored-by: Steve Macenski <[email protected]>
  • Loading branch information
@pepisg @jediofgever @SteveMacenski
3 people committed Sep 25, 2023
1 parent cffc138 commit d72b851
Show file tree
Hide file tree
Showing 19 changed files with 1,760 additions and 52 deletions.
4 changes: 3 additions & 1 deletion nav2_msgs/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -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)
Expand Down
17 changes: 17 additions & 0 deletions nav2_msgs/action/FollowGPSWaypoints.action
View file
Original file line number Diff line number Diff line change
@@ -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
1 change: 1 addition & 0 deletions nav2_msgs/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
<depend>geometry_msgs</depend>
<depend>action_msgs</depend>
<depend>nav_msgs</depend>
<depend>geographic_msgs</depend>

<member_of_group>rosidl_interface_packages</member_of_group>

Expand Down
30 changes: 28 additions & 2 deletions nav2_simple_commander/nav2_simple_commander/robot_navigator.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -67,6 +68,8 @@ def __init__(self, node_name='basic_navigator', namespace=''):
'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')
Expand Down Expand Up @@ -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):
Expand Down Expand Up @@ -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)
Expand Down
1 change: 1 addition & 0 deletions nav2_system_tests/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
11 changes: 11 additions & 0 deletions nav2_system_tests/src/gps_navigation/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
@@ -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
)
63 changes: 63 additions & 0 deletions nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py
View file
Original file line number Diff line number Diff line change
@@ -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"),
],
),
]
)
127 changes: 127 additions & 0 deletions nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml
View file
Original file line number Diff line number Diff line change
@@ -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
Loading

0 comments on commit d72b851

Please sign in to comment.