Skip to content

Commit d72b851

Browse files
pepisgjediofgeverSteveMacenski
committed
Add nav2_gps_waypoint_follower (#2814)
* 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]>
1 parent cffc138 commit d72b851

File tree

19 files changed

+1760
-52
lines changed

19 files changed

+1760
-52
lines changed

‎nav2_msgs/CMakeLists.txt

+3-1
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,7 @@ find_package(nav_msgs REQUIRED)
88
find_package(geometry_msgs REQUIRED)
99
find_package(rosidl_default_generators REQUIRED)
1010
find_package(std_msgs REQUIRED)
11+
find_package(geographic_msgs)
1112
find_package(action_msgs REQUIRED)
1213

1314
nav2_package()
@@ -49,7 +50,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
4950
"action/Spin.action"
5051
"action/DummyBehavior.action"
5152
"action/FollowWaypoints.action"
52-
DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs
53+
"action/FollowGPSWaypoints.action"
54+
DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs geographic_msgs
5355
)
5456

5557
ament_export_dependencies(rosidl_default_runtime)

‎nav2_msgs/action/FollowGPSWaypoints.action

+17
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
#goal definition
2+
uint32 number_of_loops
3+
uint32 goal_index 0
4+
geographic_msgs/GeoPose[] gps_poses
5+
---
6+
#result definition
7+
8+
# Error codes
9+
# Note: The expected priority order of the errors should match the message order
10+
uint16 NONE=0
11+
uint16 UNKNOWN=600
12+
uint16 TASK_EXECUTOR_FAILED=601
13+
14+
MissedWaypoint[] missed_waypoints
15+
---
16+
#feedback
17+
uint32 current_waypoint

‎nav2_msgs/package.xml

+1
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@
1919
<depend>geometry_msgs</depend>
2020
<depend>action_msgs</depend>
2121
<depend>nav_msgs</depend>
22+
<depend>geographic_msgs</depend>
2223

2324
<member_of_group>rosidl_interface_packages</member_of_group>
2425

‎nav2_simple_commander/nav2_simple_commander/robot_navigator.py

+28-2
Original file line numberDiff line numberDiff line change
@@ -25,7 +25,8 @@
2525
from lifecycle_msgs.srv import GetState
2626
from nav2_msgs.action import AssistedTeleop, BackUp, Spin
2727
from nav2_msgs.action import ComputePathThroughPoses, ComputePathToPose
28-
from nav2_msgs.action import FollowPath, FollowWaypoints, NavigateThroughPoses, NavigateToPose
28+
from nav2_msgs.action import FollowPath, FollowWaypoints, FollowGPSWaypoints, \
29+
NavigateThroughPoses, NavigateToPose
2930
from nav2_msgs.action import SmoothPath
3031
from nav2_msgs.srv import ClearEntireCostmap, GetCostmap, LoadMap, ManageLifecycleNodes
3132

@@ -67,6 +68,8 @@ def __init__(self, node_name='basic_navigator', namespace=''):
6768
'navigate_through_poses')
6869
self.nav_to_pose_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
6970
self.follow_waypoints_client = ActionClient(self, FollowWaypoints, 'follow_waypoints')
71+
self.follow_gps_waypoints_client = ActionClient(self, FollowGPSWaypoints,
72+
'follow_gps_waypoints')
7073
self.follow_path_client = ActionClient(self, FollowPath, 'follow_path')
7174
self.compute_path_to_pose_client = ActionClient(self, ComputePathToPose,
7275
'compute_path_to_pose')
@@ -182,6 +185,28 @@ def followWaypoints(self, poses):
182185
self.result_future = self.goal_handle.get_result_async()
183186
return True
184187

188+
def followGpsWaypoints(self, gps_poses):
189+
"""Send a `FollowGPSWaypoints` action request."""
190+
self.debug("Waiting for 'FollowWaypoints' action server")
191+
while not self.follow_gps_waypoints_client.wait_for_server(timeout_sec=1.0):
192+
self.info("'FollowWaypoints' action server not available, waiting...")
193+
194+
goal_msg = FollowGPSWaypoints.Goal()
195+
goal_msg.gps_poses = gps_poses
196+
197+
self.info(f'Following {len(goal_msg.gps_poses)} gps goals....')
198+
send_goal_future = self.follow_gps_waypoints_client.send_goal_async(goal_msg,
199+
self._feedbackCallback)
200+
rclpy.spin_until_future_complete(self, send_goal_future)
201+
self.goal_handle = send_goal_future.result()
202+
203+
if not self.goal_handle.accepted:
204+
self.error(f'Following {len(gps_poses)} gps waypoints request was rejected!')
205+
return False
206+
207+
self.result_future = self.goal_handle.get_result_async()
208+
return True
209+
185210
def spin(self, spin_dist=1.57, time_allowance=10):
186211
self.debug("Waiting for 'Spin' action server")
187212
while not self.spin_client.wait_for_server(timeout_sec=1.0):
@@ -310,7 +335,8 @@ def getResult(self):
310335

311336
def waitUntilNav2Active(self, navigator='bt_navigator', localizer='amcl'):
312337
"""Block until the full navigation system is up and running."""
313-
self._waitForNodeToActivate(localizer)
338+
if localizer != "robot_localization": # non-lifecycle node
339+
self._waitForNodeToActivate(localizer)
314340
if localizer == 'amcl':
315341
self._waitForInitialPose()
316342
self._waitForNodeToActivate(navigator)

‎nav2_system_tests/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -114,6 +114,7 @@ if(BUILD_TESTING)
114114
add_subdirectory(src/system_failure)
115115
add_subdirectory(src/updown)
116116
add_subdirectory(src/waypoint_follower)
117+
add_subdirectory(src/gps_navigation)
117118
add_subdirectory(src/behaviors/spin)
118119
add_subdirectory(src/behaviors/wait)
119120
add_subdirectory(src/behaviors/backup)

‎nav2_system_tests/src/gps_navigation/CMakeLists.txt

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
ament_add_test(test_gps_waypoint_follower
2+
GENERATE_RESULT_FOR_RETURN_CODE_ZERO
3+
COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_case_py.launch.py"
4+
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
5+
TIMEOUT 180
6+
ENV
7+
TEST_DIR=${CMAKE_CURRENT_SOURCE_DIR}
8+
TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo_gps.world
9+
GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models
10+
BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml
11+
)

‎nav2_system_tests/src/gps_navigation/dual_ekf_navsat.launch.py

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
# Copyright 2018 Open Source Robotics Foundation, Inc.
2+
# Licensed under the Apache License, Version 2.0 (the "License");
3+
# you may not use this file except in compliance with the License.
4+
# You may obtain a copy of the License at
5+
#
6+
# http://www.apache.org/licenses/LICENSE-2.0
7+
#
8+
# Unless required by applicable law or agreed to in writing, software
9+
# distributed under the License is distributed on an "AS IS" BASIS,
10+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
11+
# See the License for the specific language governing permissions and
12+
# limitations under the License.
13+
14+
from launch import LaunchDescription
15+
import launch_ros.actions
16+
import os
17+
import launch.actions
18+
19+
20+
def generate_launch_description():
21+
launch_dir = os.path.dirname(os.path.realpath(__file__))
22+
params_file = os.path.join(launch_dir, "dual_ekf_navsat_params.yaml")
23+
os.environ["FILE_PATH"] = str(launch_dir)
24+
return LaunchDescription(
25+
[
26+
launch.actions.DeclareLaunchArgument(
27+
"output_final_position", default_value="false"
28+
),
29+
launch.actions.DeclareLaunchArgument(
30+
"output_location", default_value="~/dual_ekf_navsat_example_debug.txt"
31+
),
32+
launch_ros.actions.Node(
33+
package="robot_localization",
34+
executable="ekf_node",
35+
name="ekf_filter_node_odom",
36+
output="screen",
37+
parameters=[params_file, {"use_sim_time": True}],
38+
remappings=[("odometry/filtered", "odometry/local")],
39+
),
40+
launch_ros.actions.Node(
41+
package="robot_localization",
42+
executable="ekf_node",
43+
name="ekf_filter_node_map",
44+
output="screen",
45+
parameters=[params_file, {"use_sim_time": True}],
46+
remappings=[("odometry/filtered", "odometry/global")],
47+
),
48+
launch_ros.actions.Node(
49+
package="robot_localization",
50+
executable="navsat_transform_node",
51+
name="navsat_transform",
52+
output="screen",
53+
parameters=[params_file, {"use_sim_time": True}],
54+
remappings=[
55+
("imu/data", "imu/data"),
56+
("gps/fix", "gps/fix"),
57+
("gps/filtered", "gps/filtered"),
58+
("odometry/gps", "odometry/gps"),
59+
("odometry/filtered", "odometry/global"),
60+
],
61+
),
62+
]
63+
)

‎nav2_system_tests/src/gps_navigation/dual_ekf_navsat_params.yaml

+127
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,127 @@
1+
# For parameter descriptions, please refer to the template parameter files for each node.
2+
3+
ekf_filter_node_odom:
4+
ros__parameters:
5+
frequency: 30.0
6+
two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d
7+
print_diagnostics: true
8+
debug: false
9+
publish_tf: true
10+
11+
map_frame: map
12+
odom_frame: odom
13+
base_link_frame: base_link
14+
world_frame: odom
15+
16+
odom0: odom
17+
odom0_config: [false, false, false,
18+
false, false, false,
19+
true, true, true,
20+
false, false, true,
21+
false, false, false]
22+
odom0_queue_size: 10
23+
odom0_differential: false
24+
odom0_relative: false
25+
26+
imu0: imu/data
27+
imu0_config: [false, false, false,
28+
false, false, true,
29+
false, false, false,
30+
false, false, false,
31+
false, false, false]
32+
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
33+
imu0_relative: false
34+
imu0_queue_size: 10
35+
imu0_remove_gravitational_acceleration: true
36+
37+
use_control: false
38+
39+
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,
40+
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,
41+
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,
42+
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,
43+
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,
44+
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,
45+
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,
46+
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,
47+
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,
48+
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,
49+
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,
50+
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,
51+
0.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,
52+
0.0, 0.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,
53+
0.0, 0.0, 0.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]
54+
55+
ekf_filter_node_map:
56+
ros__parameters:
57+
frequency: 30.0
58+
two_d_mode: true # Recommended to use 2d mode for nav2, since its world representation is 2d
59+
print_diagnostics: true
60+
debug: false
61+
publish_tf: true
62+
63+
map_frame: map
64+
odom_frame: odom
65+
base_link_frame: base_link
66+
world_frame: map
67+
68+
odom0: odom
69+
odom0_config: [false, false, false,
70+
false, false, false,
71+
true, true, true,
72+
false, false, true,
73+
false, false, false]
74+
odom0_queue_size: 10
75+
odom0_differential: false
76+
odom0_relative: false
77+
78+
odom1: odometry/gps
79+
odom1_config: [true, true, false,
80+
false, false, false,
81+
false, false, false,
82+
false, false, false,
83+
false, false, false]
84+
odom1_queue_size: 10
85+
odom1_differential: false
86+
odom1_relative: false
87+
88+
imu0: imu/data
89+
imu0_config: [false, false, false,
90+
false, false, true,
91+
false, false, false,
92+
false, false, false,
93+
false, false, false]
94+
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
95+
imu0_relative: false
96+
imu0_queue_size: 10
97+
imu0_remove_gravitational_acceleration: true
98+
99+
use_control: false
100+
101+
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,
102+
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,
103+
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,
104+
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,
105+
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,
106+
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,
107+
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,
108+
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,
109+
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,
110+
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,
111+
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,
112+
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,
113+
0.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,
114+
0.0, 0.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,
115+
0.0, 0.0, 0.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]
116+
117+
navsat_transform:
118+
ros__parameters:
119+
frequency: 30.0
120+
delay: 3.0
121+
magnetic_declination_radians: 0.0429351 # For lat/long 55.944831, -3.186998
122+
yaw_offset: 0.0
123+
zero_altitude: true
124+
broadcast_utm_transform: true
125+
publish_filtered_gps: true
126+
use_odometry_yaw: true
127+
wait_for_datum: false

0 commit comments

Comments
 (0)