nav2_gps_waypoint_follower_demo 不能在ros2 humble中直接使用的解决方法

GIT上的nav2_gps_waypoint_follower_demo是基于ros-iron编写的，其中followGpsWaypoints(wps) service只能在Iron上使用。

解决方法：

第一步：将interactive_waypoint_follower.py修改为如下代码：

import rclpy
from rclpy.node import Node
from nav2_simple_commander.robot_navigator import BasicNavigator
from geometry_msgs.msg import PointStamped
from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose
from nav2_msgs.action import FollowWaypoints
from geometry_msgs.msg import PoseStamped
from robot_localization.srv import FromLLclass InteractiveGpsWpCommander(Node):"""ROS2 node to send gps waypoints to nav2 received from mapviz's point click publisher"""def __init__(self):super().__init__(node_name="gps_wp_commander")self.navigator = BasicNavigator("basic_navigator")self.mapviz_wp_sub = self.create_subscription(PointStamped, "/clicked_point", self.mapviz_wp_cb, 1)self.localizer = self.create_client(FromLL,  '/fromLL')while not self.localizer.wait_for_service(timeout_sec=1.0):self.get_logger().info('Service not available, waiting again...')self.client_futures = []self.get_logger().info('Ready for waypoints...')def mapviz_wp_cb(self, msg: PointStamped):"""clicked point callback, sends received point to nav2 gps waypoint follower if its a geographic point"""if msg.header.frame_id != "wgs84":self.get_logger().warning("Received point from mapviz that ist not in wgs84 frame. This is not a gps point and wont be followed")returnwps = [latLonYaw2Geopose(msg.point.y, msg.point.x)]for wp in wps:self.req = FromLL.Request()self.req.ll_point.longitude = wp.position.longitudeself.req.ll_point.latitude = wp.position.latitudeself.req.ll_point.altitude = wp.position.altitudeself.get_logger().info("Waypoint added to conversion queue...")self.client_futures.append(self.localizer.call_async(self.req))def command_send_cb(self, future):self.resp = PoseStamped()self.resp.header.frame_id = 'map'self.resp.header.stamp = self.get_clock().now().to_msg()self.resp.pose.position = future.result().map_pointself.navigator.goToPose(self.resp)def spin(self):while rclpy.ok():rclpy.spin_once(self)incomplete_futures = []for f in self.client_futures:if f.done():self.client_futures.remove(f)self.get_logger().info("Following converted waypoint...")self.command_send_cb(f)else:incomplete_futures.append(f)self.client_futures = incomplete_futuresdef main():rclpy.init()gps_wpf = InteractiveGpsWpCommander()gps_wpf.spin()if __name__ == "__main__":main()

将logged_waypoint_follower.py改为如下代码：

import rclpy
from nav2_simple_commander.robot_navigator import BasicNavigator
import yaml
from ament_index_python.packages import get_package_share_directory
import os
import sys
import time
from robot_localization.srv import FromLL
from rclpy.node import Node
from nav2_gps_waypoint_follower_demo.utils.gps_utils import latLonYaw2Geopose
from nav2_msgs.action import FollowWaypoints
from geometry_msgs.msg import PoseStampedclass YamlWaypointParser:"""Parse a set of gps waypoints from a yaml file"""def __init__(self, wps_file_path: str) -> None:with open(wps_file_path, 'r') as wps_file:self.wps_dict = yaml.safe_load(wps_file)def get_wps(self):"""Get an array of geographic_msgs/msg/GeoPose objects from the yaml file"""gepose_wps = []for wp in self.wps_dict["waypoints"]:latitude, longitude, yaw = wp["latitude"], wp["longitude"], wp["yaw"]gepose_wps.append(latLonYaw2Geopose(latitude, longitude, yaw))return gepose_wpsclass GpsWpCommander(Node):"""Class to use nav2 gps waypoint follower to follow a set of waypoints logged in a yaml file"""def __init__(self, wps_file_path):super().__init__('minimal_client_async')self.navigator = BasicNavigator("basic_navigator")self.wp_parser = YamlWaypointParser(wps_file_path)self.localizer = self.create_client(FromLL,  '/fromLL')while not self.localizer.wait_for_service(timeout_sec=1.0):self.get_logger().info('service not available, waiting again...')def start_wpf(self):"""Function to start the waypoint following"""self.navigator.waitUntilNav2Active(localizer='controller_server')wps = self.wp_parser.get_wps()wpl = []for wp in wps:self.req = FromLL.Request()self.req.ll_point.longitude = wp.position.longitudeself.req.ll_point.latitude = wp.position.latitudeself.req.ll_point.altitude = wp.position.altitudelog = 'long{:f}, lat={:f}, alt={:f}'.format(self.req.ll_point.longitude, self.req.ll_point.latitude, self.req.ll_point.altitude)self.get_logger().info(log)self.future = self.localizer.call_async(self.req)rclpy.spin_until_future_complete(self, self.future)self.resp = PoseStamped()self.resp.header.frame_id = 'map'self.resp.header.stamp = self.get_clock().now().to_msg()self.resp.pose.position = self.future.result().map_pointlog = 'x={:f}, y={:f}, z={:f}'.format(self.future.result().map_point.x, self.future.result().map_point.y, self.future.result().map_point.z)self.get_logger().info(log)self.resp.pose.orientation = wp.orientationwpl += [self.resp]self.navigator.followWaypoints(wpl)print("wps completed successfully")def main():rclpy.init()# allow to pass the waypoints file as an argumentdefault_yaml_file_path = os.path.join(get_package_share_directory("node"), "config", "demo_waypoints.yaml")if len(sys.argv) > 1:yaml_file_path = sys.argv[1]else:yaml_file_path = default_yaml_file_pathgps_wpf = GpsWpCommander(yaml_file_path)gps_wpf.start_wpf()if __name__ == "__main__":main()

第二步：除了上述代码需要修改，nav2_no_map_params.yaml也需要修改，该文件中bt_navigator节点下的内容改为nav2 humble里面默认的部分，另外，建议global_costmap中的width/height改大，例如从50改到500，不然容易出现超出global_costmap范围的提示。

参考链接（tks LuukBerkel ）：nav2_gps_waypoint_follower_demo does not work on ros2 humble???! · Issue #77 · ros-planning/navigation2_tutorials · GitHub