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 FromLL
class 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")
return
wps = [latLonYaw2Geopose(msg.point.y, msg.point.x)]
for wp in wps:
self.req = FromLL.Request()
self.req.ll_point.longitude = wp.position.longitude
self.req.ll_point.latitude = wp.position.latitude
self.req.ll_point.altitude = wp.position.altitude
self.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_point
self.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_futures
def 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 PoseStamped
class 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_wps
class 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.longitude
self.req.ll_point.latitude = wp.position.latitude
self.req.ll_point.altitude = wp.position.altitude
log = '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_point
log = '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.orientation
wpl += [self.resp]
self.navigator.followWaypoints(wpl)
print("wps completed successfully")
def main():
rclpy.init()
# allow to pass the waypoints file as an argument
default_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_path
gps_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范围的提示。
欢迎加入多源融合定位与控制技术讨论QQ群,群号:518859469
参考链接(tks
LuukBerkel ):
nav2_gps_waypoint_follower_demo does not work on ros2 humble???! · Issue #77 · ros-planning/navigation2_tutorials · GitHub